Go to the documentation of this file.00001 #ifndef COLLISIONINTERFACE_H
00002 #define COLLISIONINTERFACE_H
00003
00004 #include <tabletop_collision_map_processing/collision_map_interface.h>
00005 #include <tabletop_collision_map_processing/TabletopCollisionMapProcessing.h>
00006 #include "cop.h"
00007 #include <ros/ros.h>
00008 #include <vision_srvs/cop_add_collision.h>
00009 #include <vision_srvs/cop_get_object_shape.h>
00010 #include "RelPoseFactory.h"
00011
00012 #include <arm_navigation_msgs/CollisionObject.h>
00013
00014
00015 #define COLLISION_PROCESSING_SERVICE_NAME "/tabletop_collision_map_processing/tabletop_collision_map_processing"
00016
00017 #define PICKUP_ACTION_NAME "/object_manipulator/object_manipulator_pickup"
00018 #define PLACE_ACTION_NAME "/object_manipulator/object_manipulator_place"
00019 #define GET_MODEL_DESCRIPTION_NAME "/objects_database_node/get_model_description"
00020
00021 namespace cop
00022 {
00023
00024 class CollisionInterface
00025 {
00026 public:
00027 CollisionInterface(cop_world &cop_reference, ros::NodeHandle &nh) : m_myCop(cop_reference)
00028 {
00029 if (!nh.ok()) exit(0);
00030 m_collision_processing_srv = nh.serviceClient<tabletop_collision_map_processing::TabletopCollisionMapProcessing>
00031 (COLLISION_PROCESSING_SERVICE_NAME, true);
00032
00033 RelPose* pose = RelPoseFactory::GetRelPose("/base_link");
00034 m_base_link_id = pose->m_uniqueID;
00035 RelPoseFactory::FreeRelPose(&pose);
00036 std::string servicename = "/cop_collision";
00037 std::string service_shape = "/cop_geometric_shape";
00038
00039 printf("n.advertiseService<cop_add_collsion>(%s, ...)\n", servicename.c_str());
00040 printf("n.advertiseService<cop_get_object_shape>(%s, ...)\n", service_shape.c_str());
00041
00042 m_copCollisionService = nh.advertiseService(servicename, &CollisionInterface::AddCollisionCB, this);
00043 m_copGeometricShapeService = nh.advertiseService(service_shape, &CollisionInterface::GetGeometricShape, this);
00044 m_object_in_map_pub = nh.advertise<arm_navigation_msgs::CollisionObject>("/collision_object", 10);
00045
00046
00047 }
00048 bool AddCollisionCB(vision_srvs::cop_add_collision::Request& msg, vision_srvs::cop_add_collision::Response& answer);
00049
00050 std::string AddCollisionObject(Signature* current_object, arm_navigation_msgs::CollisionObject &object, bool ignore_pcd=false);
00051
00052 bool GetGeometricShape(vision_srvs::cop_get_object_shape::Request &msg, vision_srvs::cop_get_object_shape::Response &answer);
00053
00054 void ResetCollisionMap(Signature* current_object);
00055
00056 private:
00057 cop_world &m_myCop;
00058
00059 ros::ServiceClient m_collision_processing_srv;
00060 ros::ServiceServer m_copCollisionService;
00061 ros::ServiceServer m_copGeometricShapeService;
00062 ros::Publisher m_object_in_map_pub;
00063 LocatedObjectID_t m_base_link_id;
00064 };
00065
00066
00067 }
00068
00069 #endif
