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