How to change position of collision object in Moveit

edit

Hi!

1. It is possible.

2. Not sure. If the object is attached to the robot through the URDF then is not possible. If you attach the object using moveit_msgs/AttachedCollisionObject.h it will move with the robot until you detach it.

Check this code I implemented, it uses the MOVE operation and works fine. I hope it will be helpful.

It listens to three different transformations published by aruco_ros package and then it updates the position of the objects in the planning scene. The robot avoids collision with these objects. The moveit_msgs::CollisionObject bin is reused since the three objects have the same mesh.

Nicolas.

#include "geometric_shapes/shapes.h"
#include "geometric_shapes/mesh_operations.h"
#include "geometric_shapes/shape_operations.h"

#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>

#include <tf/transform_listener.h>

int main(int argc, char** argv){

ros::init(argc, argv, "bins_tracker");
ros::NodeHandle node; 

tf::TransformListener listener; //tf listener & transformations
tf::StampedTransform t_bin1;
tf::StampedTransform t_bin2;
tf::StampedTransform t_bin3;

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");
object_ids.push_back("Bin2");
object_ids.push_back("Bin3");

moveit::planning_interface::PlanningSceneInterface planning_scene_interface; //planning interface
moveit_msgs::CollisionObject bin; //Create an object msg

shapes::Mesh* m = shapes::createMeshFromResource("package://motoman_sda10f_support/meshes/bin.stl"); //find mesh
shape_msgs::Mesh mesh; //create a mesh msg
shapes::ShapeMsg mesh_msg; //create a shape msg
shapes::constructMsgFromShape(m,mesh_msg); //convert shape into a shape msg
ros::Duration(0.5).sleep();
mesh = boost::get<shape_msgs::Mesh>(mesh_msg); // shape msg is assigned to mesh msg

bin.header.frame_id = "torso_base_link"; //"father frame"   
bin.meshes.resize(1); //scale
bin.meshes[0]=mesh; //mesh

bin.mesh_poses.resize(1); //vector resize
bin.mesh_poses[0].position.x = 0; //pose
bin.mesh_poses[0].position.y = 0;
bin.mesh_poses[0].position.z = 0;
bin.mesh_poses[0].orientation.w= 1.0;
bin.mesh_poses[0].orientation.x= 0;
bin.mesh_poses[0].orientation.y= 0;
bin.mesh_poses[0].orientation.z= 0;

bin.operation = bin.ADD; //add object to collitions 

bin.id =object_ids[0]; //rename object 
collision_objects.push_back(bin); //you can insert different objects using a vector of collition objects
bin.id=object_ids[1]; //rename object and reuse bin msg struture
collision_objects.push_back(bin); //add object to vector
bin.id=object_ids[2];//rename object and reuse bin msg struture
collision_objects.push_back(bin); //add object to vector

planning_scene_interface.addCollisionObjects(collision_objects); //add objects to planning interface

bin.meshes.clear(); //Clear mesh required for MOVE operation (Only to avoid a warning)
bin.operation = bin.MOVE; //change operation to MOVE

while (node.ok()){
    //Listen to tfs
    try{
        listener.lookupTransform("torso_base_link", "bin1", ros::Time(0), t_bin1);
    }

        catch (tf::TransformException ex){
        ROS_WARN("%s",ex.what());
    }

    try{
        listener.lookupTransform("torso_base_link", "bin2", ros::Time(0), t_bin2);
    }
    catch (tf::TransformException ex){
        ROS_WARN("%s",ex.what());
    }

    try{
        listener.lookupTransform("torso_base_link", "bin3", ros::Time(0), t_bin3);
    }
    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.mesh_poses[0].position.x = t_bin1.getOrigin().x(); //set pose
    bin.mesh_poses[0].position.y = t_bin1 ...