0

嗨，我想在机器人运动期间（在执行轨迹期间）向场景添加一个碰撞对象。我知道可以使用move_group节点，也可以在这里阅读move_group概念。但是，仍然需要一些帮助来构建节点。我编写了 cpp 代码，但认为仍然缺少一些东西，例如检查当前场景中碰撞的Replanning复选框和isPathValid函数，但不确定实现以及我的代码中应该在哪里。到目前为止的代码

using namespace sensor_msgs;
using namespace std;


class ObjectsCollision
{
public:
  const std::string PLANNING_GROUP = "crane_control";   
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  ObjectsCollision()
  {
    ros::NodeHandle n;
    // Initialize subscriber to the raw point cloud
    ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes", 50, &ObjectsCollision::cloudCB, this);
    // Spin
    ros::spin();
  }


void cloudCB(const darknet_ros_3d_msgs::BoundingBoxes3d& boxes)
{ 
    
 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
 moveit::planning_interface::MoveGroupInterface::Plan my_plan;
 const std::string MoveGroupInterface = "robot_description";  
  
 int counter_id = 0;
 std::vector<std::string> object_ids;
 moveit_msgs::CollisionObject collision_object;
 //collision_object.header.frame_id = "zed_left_camera_optical_frame";
 collision_object.header.frame_id = move_group.getPlanningFrame();

 collision_object.id = "BOX_";
 std::vector<moveit_msgs::CollisionObject> collision_objects;



  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.z = 0.5;
  move_group.setPoseTarget(target_pose1);
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  move_group.move();


                                 
          for(auto bb : boxes.bounding_boxes)
                    {
                    
                                  shape_msgs::SolidPrimitive primitive;
                                  primitive.type = primitive.BOX;
                                  primitive.dimensions.resize(3);
                                  string str= to_string(counter_id++);                                

                                  //Pose
                                  geometry_msgs::Pose box_pose;
                                  box_pose.position.x = abs ((bb.xmax + bb.xmin)/2);
                                  box_pose.position.y = abs ((bb.ymax + bb.ymin)/2);
                                  box_pose.position.z = abs ((bb.zmax + bb.zmin)/2);
                                  box_pose.orientation.x = 0; 
                                  box_pose.orientation.y = 0;   
                                  box_pose.orientation.z = 0;
                                  box_pose.orientation.w = 1.0;
                                
                                  //Dimension
                                  primitive.dimensions[0] = abs (bb.xmax - bb.xmin);
                                  primitive.dimensions[1] = abs (bb.ymax - bb.ymin);
                                  primitive.dimensions[2] = abs (bb.zmax - bb.zmin); 

                                  //Collision object       
                                  collision_object.primitives.push_back(primitive);
                                  collision_object.primitive_poses.push_back(box_pose);                               
                                  collision_objects.push_back(collision_object); 
                                                                    
                                 

                            }
                                 

                                 collision_object.operation = collision_object.REMOVE; 
                                 object_ids.push_back(collision_object.id);
                                 planning_scene_interface.removeCollisionObjects(object_ids); 
                                 //sleep(0.6); 
                                 collision_object.operation = collision_object.ADD;

                                 planning_scene_interface.applyCollisionObjects(collision_objects);
                                 //sleep(0.6);
                                
                                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);

                                success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
                                    
 }

};


int main(int argc, char** argv)
{
  // Initialize ROS
  ros::init(argc, argv, "collision_object");
  // Start the segmentor
  ObjectsCollision();
}
4

0 回答 0