Problems when using the "MOVE" Operation:I have a code which spawns objects into the planning scene using the "ADD" operation. However, whenever I try to use "MOVE" my code fails. Anybody experienced a similar error while using the "MOVE" operation?
I am detecting an aruco marker and spawn an object at the detected pose. Inside the planning scene, I have a robot arm (panda arm) and my objects will be spawned next to the robot. My code works perfectly fine, whenever I use the "ADD" Operation for my collision objects. However, as soon as I start using the "MOVE" operation, the object just get's moved into the origin inside the base of my robot (either it gets spawned inside the origin, or it get's stuck inside the robot somehow). I am clueless on how this happens and wonder if anybody experienced a similar error while using the "MOVE" operation?
Here is my code. It was inspired by another thread: https://answers.ros.org/question/276194/how-to-change-position-of-collision-object-in-moveit/:
int main(int argc, char** argv){
ros::init(argc, argv, "bins_tracker");
ros::NodeHandle node;
ros::AsyncSpinner spinner(1);
spinner.start();
// Name of the planning Group
static const std::string PLANNING_GROUP = "panda_arm";
//Use MoveGroupInterface
moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);
tf::TransformListener listener; //tf listener & transformations
tf::StampedTransform t_bin1;
std::vector<moveit_msgs::CollisionObject> collision_objects; //vector of objects
std::vector<std::string> object_ids; //vector of strings (names of objects)
object_ids.push_back("Bin1");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; //planning interface
ros::Duration(2.0).sleep();
moveit_msgs::CollisionObject bin; //Create an object msg
bin.header.frame_id = move_group_interface.getPlanningFrame();
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 0.05;
primitive.dimensions[2] = 0.03;
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.orientation.x= 0;
box_pose.orientation.y= 0;
box_pose.orientation.z= 0;
box_pose.position.x = 0.0;
box_pose.position.y = 0.0;
box_pose.position.z = 1.1;
bin.primitives.push_back(primitive);
bin.primitive_poses.push_back(box_pose);
bin.operation = bin.ADD; //add object to collisions
bin.id =object_ids[0]; //rename object
collision_objects.push_back(bin); //you can insert different objects using a vector of collision objects
ros::Duration(2.0).sleep();
planning_scene_interface.addCollisionObjects(collision_objects); //add objects to planning interface
ros::Duration(2.0).sleep();
bin.primitives.clear(); //Clear mesh required for MOVE operation (Only to avoid a warning)
bin.operation = bin.MOVE; //change operation to MOVE
//works perfectly fine whenever I use "bin.ADD"
//check whether frame is being published, debugging purpose
ros::Duration timeout(5.0); // Timeout after 5 seconds
if (listener.waitForTransform("world", "aruco_marker_frame", ros::Time(0), timeout)) {
ROS_INFO("Transform between world and aruco_marker_frame is available.");
} else {
ROS_WARN("Timeout waiting for transform between world and aruco_marker_frame.");
return 1; // Exit if the transform is not available within the timeout duration
}
double desired_frequency = 10.0; // 10 Hz
ros::Rate loop_rate(desired_frequency);
while (node.ok()){
//Listen to tfs
try{
// Wait for the "world" frame to become available
listener.lookupTransform("world", "aruco_marker_frame", ros::Time(0), t_bin1);
ROS_INFO("here is the pose %f", t_bin1.getOrigin().x());
}
catch (tf::TransformException ex){
ROS_WARN("%s",ex.what());
}
collision_objects.clear(); //clear previous data in the vector
bin.id = object_ids[0]; //bin 1
bin.primitive_poses[0].position.x = t_bin1.getOrigin().x(); //set pose
bin.primitive_poses[0].position.y = t_bin1.getOrigin().y();
bin.primitive_poses[0].position.z = t_bin1.getOrigin().z();
bin.primitive_poses[0].orientation.w= t_bin1.getRotation().w();
bin.primitive_poses[0].orientation.x= t_bin1.getRotation().x();
bin.primitive_poses[0].orientation.y= t_bin1.getRotation().y();
bin.primitive_poses[0].orientation.z= t_bin1.getRotation().z();
collision_objects.push_back(bin); // add to vector
planning_scene_interface.applyCollisionObjects(collision_objects); //apply changes to planning interface
loop_rate.sleep();
}
planning_scene_interface.removeCollisionObjects(object_ids); //delete objects from planning interface*/
ros::shutdown(); //turn off
return 0;
}
Asked by NikolaP1997 on 2023-05-17 04:41:40 UTC
Comments
I notice that the noetic planning_scene tutorial uses a different API compared to your older-style code (for both the object types and the use of a planning_scene.world property):
https://ros-planning.github.io/moveit_tutorials/doc/planning_scene_ros_api/planning_scene_ros_api_tutorial.html
Asked by Mike Scheutzow on 2023-05-19 08:24:38 UTC