21 #include <fcl/shape/geometric_shapes.h> 33 int main(
int argc,
char** argv)
35 ros::init(argc, argv,
"cob_obstacle_distance");
41 ROS_ERROR(
"Failed to initialize DistanceManager.");
74 fcl::Box b(0.1, 0.1, 0.1);
77 "package://cob_gazebo_objects/Media/models/milk.dae",
void addObstacle(const std::string &id, PtrIMarkerShape_t s)
int main(int argc, char **argv)
std::shared_ptr< IMarkerShape > PtrIMarkerShape_t
const std::string getRootFrame() const
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void jointstateCb(const sensor_msgs::JointState::ConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
Template class implementation for box, sphere and cylinder fcl::shapes. Creates visualization marker...
geometry_msgs::TransformStamped t
bool registerLinkOfInterest(cob_srvs::SetString::Request &request, cob_srvs::SetString::Response &response)
#define ROS_INFO_STREAM(args)
void addTestObstacles(DistanceManager &dm)
void registerObstacle(const moveit_msgs::CollisionObject::ConstPtr &msg)
ROSCPP_DECL void spinOnce()