Go to the documentation of this file.00001
00002
00003 #include <ros/ros.h>
00004 #include <std_srvs/SetBool.h>
00005 #include <geometry_msgs/PoseStamped.h>
00006 #include <geometry_msgs/Pose.h>
00007 #include <ar_track_alvar_msgs/AlvarMarkers.h>
00008 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00009 #include <moveit_msgs/CollisionObject.h>
00010 #include <iostream>
00011 #include <sstream>
00012 #include "tf/tf.h"
00013
00014
00015 bool update_cb(std_srvs::SetBool::Request &req,std_srvs::SetBool::Response &res);
00016 void handle_objects(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& markers);
00017 std::string get_obj_id(int id);
00018 shape_msgs::SolidPrimitive get_obj_shape(int id);
00019 geometry_msgs::Pose get_obj_position(int id,geometry_msgs::Pose pose);
00020
00021 bool update=false;
00022
00023
00024
00025 moveit::planning_interface::PlanningSceneInterface *planning_scene_interface_ptr;
00026
00027
00028
00029
00030 int main(int argc, char **argv) {
00031 ros::init(argc, argv, "objects_handler");
00032 ros::NodeHandle n;
00033 ros::NodeHandle pn("~");
00034
00035
00036 std::string db_path;
00037 pn.param<std::string>("db_path", db_path, "??");
00038
00039 ros::Subscriber ar_sub = n.subscribe("ar_pose_marker", 10, handle_objects);
00040 ros::Subscriber color_sub = n.subscribe("detected_objects", 10, handle_objects);
00041
00042
00043 ros::ServiceServer service = n.advertiseService("update_collision_objects", update_cb);
00044
00045
00046 moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
00047 planning_scene_interface_ptr=&planning_scene_interface;
00048
00049 while (ros::ok()){
00050
00051
00052
00053 ros::spinOnce();
00054 }
00055 return 0;
00056 }
00057
00058 std::string get_obj_id(int id) {
00059
00060
00061
00062
00063
00064
00065 if (id==1) return "can";
00066 else return "table";
00067
00068 }
00069
00070 shape_msgs::SolidPrimitive get_obj_shape(int id) {
00071
00072 shape_msgs::SolidPrimitive object_primitive;
00073 if (id==1) {
00074 object_primitive.type = object_primitive.CYLINDER;
00075 object_primitive.dimensions.resize(2);
00076 object_primitive.dimensions[0] = 0.17;
00077 object_primitive.dimensions[1] = 0.03;
00078 }
00079 else {
00080 object_primitive.type = object_primitive.BOX;
00081 object_primitive.dimensions.resize(3);
00082 object_primitive.dimensions[0] = 0.3;
00083 object_primitive.dimensions[1] = 0.3;
00084 object_primitive.dimensions[2] = 0.01;
00085 }
00086 return object_primitive;
00087 }
00088
00089 geometry_msgs::Pose get_obj_position(int id,geometry_msgs::Pose pose){
00090
00091
00092 return pose;
00093 }
00094
00095 void handle_objects(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& markers)
00096 {
00097
00098 std::vector<moveit_msgs::CollisionObject> col_objects;
00099
00100 for (int i=0;i<markers->markers.size();i++) {
00101
00102 ar_track_alvar_msgs::AlvarMarker m=markers->markers[i];
00103
00104 moveit_msgs::CollisionObject obj;
00105 obj.header.frame_id = m.header.frame_id;
00106 obj.id = get_obj_id(m.id);
00107 obj.primitives.push_back(get_obj_shape(m.id));
00108
00109 obj.primitive_poses.push_back(get_obj_position(m.id,m.pose.pose));
00110 obj.operation=obj.ADD;
00111
00112 col_objects.push_back(obj);
00113
00114 if (update) planning_scene_interface_ptr->addCollisionObjects(col_objects);
00115
00116 }
00117 }
00118
00119
00120 bool update_cb(std_srvs::SetBool::Request &req,
00121 std_srvs::SetBool::Response &res) {
00122
00123 update=req.data;
00124 res.success=true;
00125 if (update) res.message="update collision is ON";
00126 else res.message="update collision is OFF";
00127 return true;
00128 }