box.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <kdl/frames.hpp>
00003 #include <arm_navigation_msgs/CollisionObject.h>
00004 #include <arm_navigation_msgs/Shape.h>
00005 
00006 int main(int argc, char** argv) {
00007 
00008   ros::init(argc, argv, "box");
00009 
00010   ros::NodeHandle nh;
00011 
00012   ros::Publisher object_in_map_pub_;
00013   object_in_map_pub_  = nh.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 10);
00014 
00015   sleep(2);
00016 
00017   //add the cylinder into the collision space
00018   arm_navigation_msgs::CollisionObject cylinder_object;
00019   cylinder_object.id = "/boxs";
00020   // COllision Operation
00021   // ADD
00022   //REMOVE
00023   // DETACH_AND_ADD_AS_OBJECT
00024   //ATTACH_AND_REMOVE_AS_OBJECT
00025   cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00026   cylinder_object.header.frame_id = "/world";
00027   cylinder_object.header.stamp = ros::Time::now();
00028   arm_navigation_msgs::Shape object;
00029   //shape type
00030   //CYLINDER
00031   //SPHERE
00032   //BOX
00033   //MESH
00034   object.type = arm_navigation_msgs::Shape::BOX;
00035 
00036 // the origin of each shape is considered at the shape's center
00037 
00038 // for sphere
00039 // radius := dimensions[0]
00040 
00041 //for cylinder
00042 // radius := dimensions[0]
00043 // length := dimensions[1]
00044 // the length is along the Z axis
00045 
00046 // for box
00047 // size_x := dimensions[0]
00048 // size_y := dimensions[1]
00049 // size_z := dimensions[2]
00050 
00051  /* object.dimensions.resize(2);
00052   object.dimensions[0] = 0.02;
00053   object.dimensions[1] = 1.5;
00054   geometry_msgs::Pose pose;
00055   pose.position.x = 0.5;
00056   pose.position.y = 0.14;
00057   pose.position.z = 0.75;
00058   pose.orientation.x = 0;
00059   pose.orientation.y = 0;
00060   pose.orientation.z = 0;
00061   pose.orientation.w = 1;*/
00062   object.dimensions.resize(3);
00063   object.dimensions[0] = 0.25;
00064   object.dimensions[1] = 0.25;
00065   object.dimensions[2] = 0.5;
00066   geometry_msgs::Pose pose;
00067   pose.position.x = 0.53;
00068   pose.position.y = 0.14;
00069   pose.position.z = 0.75;
00070   pose.orientation.x = 0;
00071   pose.orientation.y = 0;
00072   pose.orientation.z = 0;
00073   pose.orientation.w = 1;
00074   cylinder_object.shapes.push_back(object);
00075   cylinder_object.poses.push_back(pose);
00076 
00077   object_in_map_pub_.publish(cylinder_object);
00078 
00079   ROS_INFO("Should have published");
00080 
00081   ros::Duration(2.0).sleep();
00082 
00083   ros::shutdown();
00084 
00085 }


iri_wam_arm_navigation
Author(s): IRI Robotics Lab, Ivan Rojas (ivan.rojas.j@gmail.com)
autogenerated on Fri Dec 6 2013 22:32:00