0
$\begingroup$

Rosanswers logo

Hi

I have successfully introduced collision objects in the Moveit planning scene. The collision objects are real objects detected with a ZED camera and they are in the Moveit simulation as can see from this code and RVIZ screenshot. Here the collision objects code

using namespace sensor_msgs;
using namespace std;

void chatterCallback(const darknet_ros_3d_msgs::BoundingBoxes3d& boxes)
{ 
         
 int counter_id = 0;
 const std::string PLANNING_GROUP = "crane_control";    
 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
 std::vector<std::string> object_ids;
 moveit_msgs::CollisionObject collision_object;
 collision_object.header.frame_id = "world";
 collision_object.id = "BOX_";
 std::vector<moveit_msgs::CollisionObject> collision_objects;
                                 
          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)-10;
                                  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 objects                                                                                                                               
                                  collision_object.primitives.push_back(primitive);
                                  collision_object.primitive_poses.push_back(box_pose);                                                                   
                                  collision_objects.push_back(collision_object); 
                                  sleep(0.5);                                                        

                            }
                                 collision_object.operation = collision_object.REMOVE; 
                                 object_ids.push_back(collision_object.id);
                                 planning_scene_interface.removeCollisionObjects(object_ids); 
                                 sleep(0.3); 
                                 collision_object.operation = collision_object.ADD;
                                 planning_scene_interface.applyCollisionObjects(collision_objects);

 }

int main(int argc,  char** argv)

{
  ros::init(argc, argv, "cpp_subscriber");
  ros::NodeHandle n;
  ros::Subscriber sub = n.subscribe("/darknet_ros_3d/bounding_boxes", 5, chatterCallback);
  ros::spin();  
  
}

Here can see the collision object with RVIZ RVIZ

So, now first I would like to give the robot a path-goal from point A to B and then when is in the collision with the object just to stop. Any Help?

Thanks

Originally posted by Astronaut on ROS Answers with karma: 330 on 2021-03-29

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

I'm not sure I understood your pipeline completely (what's a "path-goal"?), but:

  1. If you set a pose goal and requested a motion plan, your path will not be in collision that are currently in the scene.
  2. If you are requesting a cartesian path, the current interface returns the up to the first collision.
  3. If you are detecting collision objects while moving, IIRC the Replanning checkbox in the Motion Planning plugin is supposed to keep checking the trajectory for collisions and replan when it finds a collision in the remaining path. You could take a look at and change this functionality to just stop.
  4. If "path-goal" means that you already have a trajectory and you want to check it for collisions in your current scene, you can use the isPathValid functions in the PlanningScene API.

Originally posted by fvd with karma: 2180 on 2021-03-29

This answer was ACCEPTED on the original site

Post score: 2

Original comments

Comment by gvdhoorn on 2021-03-29:
And a general comment: you might want to move the initialisation of the PlanningSceneInterface and related infrastructure to somewhere outside the callback. The code as-is leads to (potentially) high-frequency creation and destruction of objects with a high cost to that creation and destruction. That's far from optimal, and will likely influence application performance quite a bit.

Comment by Astronaut on 2021-03-29:
"Path-goal " , sorry I mean navigate the robot from a point A to B. In my case the tower crane (the load is moving from point A to B and in that way I want to introduce the collision object and make the robot(tower crane) stop. Hope now more clear

Comment by Astronaut on 2021-03-29:
Like where to move the initialization of the PlanningSceneInterface and related infrastructure to somewhere outside the callback? Then what will be in the callback?

Comment by fvd on 2021-03-29:
It's not clear to me at what time you add the collision object to the scene. If it's before the planning starts, then you should be fine. If it's during movement (during execution of the trajectory) then you need to take some extra care.

Comment by Astronaut on 2021-03-29:
It's during movement. So how to proceed in this case? Which of your 4 statements can be used ?

Comment by fvd on 2021-03-29:
Mainly 3. I haven't looked at that part of the code in detail, but I know that your use case was the intention behind it. Have a look at plan_execution.cpp and the replanning flag.

You could also use 4 to check for collisions when you detect new collision objects, and stop the execution yourself.

Comment by Astronaut on 2021-03-29:
Is it in Motion Planning Pipeline? This one http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.html

Comment by fvd on 2021-03-29:
It's here. Watch out that you're not looking at outdated tutorials (you linked to Kinetic). Also please accept this answer so it's out of the queue.

Comment by Astronaut on 2021-03-29:
I can not see the replanning flag. Please can you point it out in the plan_execution.cp?

Comment by fvd on 2021-03-29:
It is in the move_group_interface and in the RViz plugin. I recommend downloading the repository and searching through all files to get a better idea of the code (try VSCode and Ctrl-Shift-F).

Comment by Astronaut on 2021-03-29:
ok. Got it. Thanks. Only one more thing about the gvdhoorn comment"to move initialisation of the PlanningSceneInterface and related infrastructure to somewhere outside the callback" Where should I move it? In the main or some Public or private class?

Comment by fvd on 2021-03-29:
I would keep the PSI as a member variable of a class.

Comment by Astronaut on 2021-03-29:
ok got thanks

Comment by Astronaut on 2021-04-18:
Sorry, one more thing. I realize that plan_execution.cpp is not in move_group_interface . So confusing, which repository to download. Please can you tell me exactly? Because plan_execution.cpp is in plan_execution. Am I missing something?

Comment by fvd on 2021-04-18:
All of those files are in the MoveIt repository. The replan flag is here in the MoveGroupInterface, as I mentioned. It is sent to the move_action_client here, which is received here, and then executed here, which refers to the code in plan_execution. It's not a straight forward structure, but it will be easier if you set up your IDE to navigate the codebase.

Comment by Astronaut on 2021-04-19:
Ok thats why better use VSCode and navigate through it with Ctrl-Shift-F, right?

Comment by fvd on 2021-04-19:
You can set it up so that you can Ctrl-Click on functions. See the tutorials for VSCode and its ROS extension.

Comment by Astronaut on 2021-04-21:
Sorry , so for my understanding I need to write four nodes (4 .cpp nodes) for the replan flag, move_action_client, receiving, and executing accordingly? Is that correct?

Comment by fvd on 2021-04-21:
No, please read about how the move_group works and post a proper question about it if you need more help.

Comment by Astronaut on 2021-04-23:
Ok. I will post a proper question as i need a more help

Comment by Astronaut on 2021-04-27:
so, can I ask this here or should post new question? Because for my case const std::string PLANNING_GROUP = "crane_control" then also moveit::planning_interface::MoveGroupInterface group("crane_control");. Is that correct?

Comment by fvd on 2021-04-27:
No offense, but I think it would be best if you spent more time on finding the solution to this sort of question on your own. In your case, it takes no more than trying it out. Consider also that you are on a free Q&A site full of volunteers, which needs everyone's involvement to function properly. Your profile says that you have posted 164 questions and only 12 answers (even fewer of which are actual answers). Have you considered answering some questions and helping out some newcomers in return? How about subscribing to some tags (strongly recommended) and giving back to the community?

Comment by Astronaut on 2021-04-27:
ok. will answer some questions and help as much I can

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.