SafetyAwarePlanner.cpp

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include "SafetyEnvironmentCreator.cpp"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "CONSTRAINT PLANNER");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    const robot_state::JointModelGroup* joint_model_group =
            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    // Visualization Setup
    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
    //visual_tools.deleteAllMarkers();
    visual_tools.loadRemoteControl();
    visual_tools.setAlpha(0.5);

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
    text_pose.translation().z() = 1.75;
    visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
    visual_tools.trigger();

    // Getting Basic Information
    ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
    ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
    ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
              std::ostream_iterator<std::string>(std::cout, ", "));

    shape_msgs::SolidPrimitive safetybox;
    geometry_msgs::Pose box_pose;
    safetybox.type = safetybox.BOX;
    safetybox.dimensions.resize(3);

    safetybox.dimensions[0] = 2.0;
    safetybox.dimensions[1] = 2.0;
    safetybox.dimensions[2] = 2.0;

    // Safety Box
    SafetyEnvironmentCreator sec(safetybox, box_pose);
    std::vector<moveit_msgs::CollisionObject> collision_objects = sec.createCentralizedSafetyEnvironment(move_group);

    //visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
    // visual_tools.setAlpha(0.0);
    // Now, let's add the collision object into the world
    ROS_INFO_NAMED("tutorial", "Add an object into the world");

    planning_scene_interface.addCollisionObjects(collision_objects);

    // Show text in RViz of status
    visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
    visual_tools.trigger();
    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");