attached object doesn't appear in rviz
ros groovy/ ubuntu 12.04
I'm working with moveit and I try to use "PlanningSceneMonitor" class. The goal is to attach an object to the hand of pr2 and then move the arm in some random pose. This is my problem:
if I call planning_scene_monitor_ptr->startStateMonitor() (which is subscribed to "attached_collision_object" topic), attached object doesn't appear in rviz.
ros::init(argc,argv,"state_display_scene_monitor");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::Rate loop_rate(1);
ros::NodeHandle node_handle;
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_ptr;
planning_scene_monitor_ptr.reset(new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
planning_scene_monitor_ptr->startSceneMonitor();
planning_scene_monitor_ptr->startStateMonitor();
planning_scene_monitor_ptr->startWorldGeometryMonitor();
robot_model::RobotModelConstPtr kinematic_model= planning_scene_monitor_ptr->getRobotModel();
robot_state::RobotStatePtr current_robot_state(new robot_state::RobotState(kinematic_model)) ;
robot_state::JointStateGroup* joint_state_group= current_robot_state->getJointStateGroup("right_arm");
ros::Publisher robot_state_publisher= node_handle.advertise<moveit_msgs::DisplayRobotState>("display_robot_state",1);
/* ATTACH AN OBJECT*/
/* First advertise and wait for a connection*/
ros::Publisher attached_object_publisher = node_handle.advertise<moveit_msgs::AttachedCollisionObject>("attached_collision_object", 1);
while(attached_object_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
/* Define the attached object message*/
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "r_wrist_roll_link";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "r_wrist_roll_link";
/* The id of the object */
attached_object.object.id = "box";
/* A default pose */
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 1.1;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.1;
attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);
/* An attach operation requires an ADD */
attached_object.object.operation = attached_object.object.ADD;
/* Publish and sleep (to view the visualized results)*/
attached_object_publisher.publish(attached_object);
ros::WallDuration sleep_time(10.0);
sleep_time.sleep();
while (ros::ok())
{
joint_state_group->setToRandomValues();
moveit_msgs::DisplayRobotState msg;
robot_state::robotStateToRobotStateMsg(*current_robot_state,msg.state);
robot_state_publisher.publish<moveit_msgs::DisplayRobotState>(msg);
ros::spinOnce();
loop_rate.sleep();
}