Hi.
I'm working with rviz and moveit built from source for the kinetic-devel branch. For the rest of the stack, I'm using the binary packages. OS is Ubuntu xenial.
When I attach objects to the robot arm, it does not show in rviz.
I'm having trouble with a robot model I created from scratch, but the same thing can be reproduced by the moveit "ROS API Planning Scene Tutorial" at http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/planning_scene_ros_api_tutorial.html
Here is an excerpt from stdout (with line numbers manually added) when I run
roslaunch moveit_tutorials planning_scene_ros_api_tutorial.launch :
1 [ INFO] [1514083362.221299219]: Adding the object into the world at the location of the right wrist.
2 [ INFO] [1514083372.229633700]: Attaching the object to the right wrist and removing it from the world.
3 [ERROR] [1514083372.230641932]: Found empty JointState message
4 [ INFO] [1514083372.230957243]: Removing world object with the same name as newly attached object: 'box'
5 [ INFO] [1514083372.231715610]: Attached object 'box' to link 'r_wrist_roll_link'
6 [ INFO] [1514083382.229897324]: Detaching the object from the robot and returning it to the world.
7 [ INFO] [1514083382.230552447]: Detached object 'box' from link 'r_wrist_roll_link' and added it back in the collision world
8 [ INFO] [1514083392.230129313]: Removing the object from the world.
Here is what happens on the rviz screen:
- At line 1, a green box gets shown at the wrist location of PR2
- At line 5, the box disappears. Expected behavior is a color change of the box. Green is used for collision objects, and purple(150,50,150) is used for attached objects. The box should turn purple.
- At line 7, the green box appears again, as expected.
I have read the following page that seems to be related, but I am not sure if it is:
https://github.com/ros-planning/moveit/issues/512
The thread says that an incorrect combination of rviz checkboxes make the attached object appear, but in my case none of the possible combination made the object appear. In any case, the fix discussed in this thread is merged to kinetic-devel, and I am building kinetic-devel .
Is there something I am missing?
--- self follow-up ---
It seems that the planning scene update message is not correctly propagated from the moveit node to the rviz node.
Here is what I did:
I used rqt_graph to study the topic names that are used to communicate the planning scene messages, and learned that
- "/planning_scene" is used for communication from the move_group client (the application) to the move_group node.
- "/move_group/monitored_planning_scene" is used for communication from the move_group node to the rviz node.
I set up 2 terminals running rostopic echo for the two topics. When my application sends out a message to attach an object in the world to the robot, it is seen in the /planning_scene topic, but the corresponding information is missing in the /move_group/monitored_planning_scene topic. It seems that the /planning_scene message is triggering a /move_group/monitored_planning_scene message, but the "attached_collision_objects" array is not propagated.
I expect that the information is relayed through this message. With what I see, there is no way for rviz to know about the attached object.
Is something going wrong here?
-- second self follow-up --
I found a bug in the move_group code where the robot state gets converted to the message tree. A function that converts a vector of AttachedBodys to a vector of AttachedCollisionObject, ---that is, from the in-process form into the message form--- has its signature wrong. The author missed to add a & to make the result vector passed by reference.
diff --git a/moveit_core/robot_state/include/moveit/robot_state/conversions.h b/moveit_core/robot_state/include/moveit/robot_state/conversions.h
index 355a643..8b2167b 100644
--- a/moveit_core/robot_state/include/moveit/robot_state/conversions.h
+++ b/moveit_core/robot_state/include/moveit/robot_state/conversions.h
@@ -91,7 +91,7 @@ void robotStateToRobotStateMsg(const RobotState& state, moveit_msgs::RobotState&
*/
void attachedBodiesToAttachedCollisionObjectMsgs(
const std::vector<const AttachedBody*>& attached_bodies,
- std::vector<moveit_msgs::AttachedCollisionObject> attached_collision_objs);
+ std::vector<moveit_msgs::AttachedCollisionObject> &attached_collision_objs);
/**
* @brief Convert a MoveIt! robot state to a joint state message
* @param state The input MoveIt! robot state object
diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp
index 07aeb8b..4559075 100644
--- a/moveit_core/robot_state/src/conversions.cpp
+++ b/moveit_core/robot_state/src/conversions.cpp
@@ -401,7 +401,7 @@ void moveit::core::robotStateToRobotStateMsg(const RobotState& state, moveit_msg
void moveit::core::attachedBodiesToAttachedCollisionObjectMsgs(
const std::vector<const AttachedBody*>& attached_bodies,
- std::vector<moveit_msgs::AttachedCollisionObject> attached_collision_objs)
+ std::vector<moveit_msgs::AttachedCollisionObject> &attached_collision_objs)
{
attached_collision_objs.resize(attached_bodies.size());
for (std::size_t i = 0; i < attached_bodies.size(); ++i)
With the above fix, I got the expected behavior.
Originally posted by hideo67 on ROS Answers with karma: 16 on 2017-12-23
Post score: 0