00001
00059 #include <ros/ros.h>
00060
00061 #include <mapping_msgs/CollisionObject.h>
00062 #include <geometric_shapes_msgs/Shape.h>
00063
00064 int main(int argc, char** argv) {
00065
00066 ros::init(argc, argv, "addCylinder");
00067
00068 ros::NodeHandle nh;
00069
00070 ros::Publisher object_in_map_pub_;
00071 object_in_map_pub_ = nh.advertise<mapping_msgs::CollisionObject>("collision_object", 10);
00072
00073 ros::Duration(2.0).sleep();
00074
00075
00076 mapping_msgs::CollisionObject cylinder_object;
00077 cylinder_object.id = "pole";
00078 cylinder_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00079
00080 cylinder_object.header.frame_id = "/base_footprint";
00081 cylinder_object.header.stamp = ros::Time::now();
00082 geometric_shapes_msgs::Shape object;
00083 object.type = geometric_shapes_msgs::Shape::CYLINDER;
00084 object.dimensions.resize(2);
00085 object.dimensions[0] = 0.1;
00086 object.dimensions[1] = 1.2;
00087 geometry_msgs::Pose pose;
00088 pose.position.x = -.1;
00089 pose.position.y = -.8;
00090 pose.position.z = .75;
00091 pose.orientation.x = 0;
00092 pose.orientation.y = 0;
00093 pose.orientation.z = 0;
00094 pose.orientation.w = 1;
00095 cylinder_object.shapes.push_back(object);
00096 cylinder_object.poses.push_back(pose);
00097
00098 cylinder_object.id = "pole";
00099 object_in_map_pub_.publish(cylinder_object);
00100
00101 ROS_INFO("Should have published");
00102
00103 ros::Duration(2.0).sleep();
00104
00105 ros::shutdown();
00106 }
00107
