Ask Your Question
0

attached object doesn't appear in rviz

asked 2013-07-03 23:44:55 -0500

Behnam Asadi gravatar image

updated 2014-11-22 17:05:08 -0500

ngrennan gravatar image

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();
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-09-26 09:58:56 -0500

isucan gravatar image

There were some bugs recently fixed related to this problem. Can you check if this is still the case?

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-07-03 23:44:55 -0500

Seen: 533 times

Last updated: Sep 26 '13