Skip to content
Snippets Groups Projects
Select Git revision
  • d6b9518be83ff0cb37015399ada1691f3c8b24a0
  • master default
  • simulated_grasping
  • v1.0
4 results

SampleConstraintPlanner.cpp

Blame
  • SebastianEbert's avatar
    Sebastian Ebert authored
    d6b9518b
    History
    SampleConstraintPlanner.cpp 4.32 KiB
    //
    // Created by sebastian on 27.03.20.
    //
    #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 <moveit/trajectory_processing/iterative_time_parameterization.h>
    #include <trajectory_msgs/JointTrajectoryPoint.h>
    
    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();
    
        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, ", "));
    
        // Define a collision object ROS message.
        moveit_msgs::CollisionObject collision_object;
        collision_object.header.frame_id = move_group.getPlanningFrame();
    
        // The id of the object is used to identify it.
        collision_object.id = "box1";
    
        // Define a box to add to the world.
        shape_msgs::SolidPrimitive primitive;
        primitive.type = primitive.BOX;
        primitive.dimensions.resize(3);
        primitive.dimensions[0] = 0.4;
        primitive.dimensions[1] = 0.1;
        primitive.dimensions[2] = 0.4;
    
        // Define a pose for the box (specified relative to frame_id)
        geometry_msgs::Pose box_pose;
        box_pose.orientation.w = 1.0;
        box_pose.position.x = 0.4;
        box_pose.position.y = -0.2;
        box_pose.position.z = 1.0;
    
        collision_object.primitives.push_back(primitive);
        collision_object.primitive_poses.push_back(box_pose);
        collision_object.operation = collision_object.ADD;
    
        std::vector<moveit_msgs::CollisionObject> collision_objects;
        collision_objects.push_back(collision_object);
    
        // Now, let's add the collision object into the world
        ROS_INFO_NAMED("constraint_planner", "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");
    
        // Now when we plan a trajectory it will avoid the obstacle
        move_group.setStartState(*move_group.getCurrentState());
        geometry_msgs::Pose another_pose;
        another_pose.orientation.w = 1.0;
        another_pose.position.x = 0.4;
        another_pose.position.y = -0.4;
        another_pose.position.z = 0.9;
        move_group.setPoseTarget(another_pose);
    
        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
    
        visual_tools.prompt("Press 'next' to move the simulated robot.");
        visual_tools.trigger();
    
        // Move the simulated robot in gazebo
        move_group.move();
    
        ros::shutdown();
        return 0;
    }