Add mobil object in collision_map to arm_navigation

edit

Hi everybody,

Situation I use the package arm_navigation with ros-electric to control a arm robot with seven degree of freedom. I can send a articular or Cartesian goal, the robot moves well and avoid the object from urdf file like the table where is the robot. I use the "robot_name"_arm_navigation.launch and a robot simulation which receive joint state y publish TF of urdf with the good position.

Problematic I would use the kinect to add man body in the scene and more particularly at the collision map for the robot can avoid the body. I want to actualize it everytime. I tried three solutions. I want add the body as several cylinders which moves in "real time".

Solution 1: I followed the tutorial Adding known objects to the collision environment to add cylinder from kinect information. In this code I just add in the topic collision_object 10 cylinders to modeling the body.

Results 1: I succeeded to add correctly all cylinder for each body part. For example a arm it's modeling by a cylinder. The robot avoids well the body but just at the beginning of his movement. During the trajectory cylinders doesn't move and I can't check collisions.

Question 1: How actualize the collision map for see cylinders moves ?

Solution 2: It's a similar solution with the tutorial Adding Virtual Objects to the Planning Scene. The difference it's only that I don't use a topic to add objects but a service. I don't see others differences with the solution 1.

Result 2: With this solution cylinders can move everytime. There isn't the same problem of solution 1. But There is a more important problem with it. This solution works only when I choose a coordinate system already present in the urdf and doesn't work with a coodinate system generated by Kinect like right_arm_1. I explain me, when I add a cylinder, I add in a coordinate system(TF) and in this case it's more easy to add cylinder directly in coordinate system of the body. And with right_arm_1 or openni_depth_frame as coordinate system the cylinders doesn't fix in this coordinate system but works perfectly with coordinate system created from urdf file. I added a link between a urdf coordinate system and openni_depth_frame but it's like if there isn't this link.

Question 2: Why the environment_server doesn't understand the new TF of Kinect system ? How fix this ?

Solution 3: We thought to add the body man directly in the urdf and modify directly the tf with a publication in /tf.

Result3: it doesn't work because the simulation publish every time all urdf tf by /robot_st_pub. Only we can modify joint state. And it's more complicated to calculate joint state of the boy.

Question 3: It's possible to use urdf to add and modify mobil object ?

thank you for your help. if I should add details ask me.

EDIT1: Hi, Thanks for ... (more)