21 #include <visualization_msgs/MarkerArray.h> 22 #include <cob_control_msgs/ObstacleDistances.h> 35 marker_pub_ = this->nh_.
advertise<visualization_msgs::MarkerArray>(
"obstacle_distance/distance_markers", 1,
true);
46 visualization_msgs::MarkerArray marker_array;
48 for(
unsigned int i = 0; i < msg->distances.size(); i++)
50 cob_control_msgs::ObstacleDistance info = msg->distances[i];
53 visualization_msgs::Marker marker_vector;
54 marker_vector.type = visualization_msgs::Marker::ARROW;
56 marker_vector.action = visualization_msgs::Marker::ADD;
57 marker_vector.ns =
"arrows";
58 marker_vector.id = 2*i;
59 marker_vector.header.frame_id = info.header.frame_id;
61 marker_vector.scale.x = 0.01;
62 marker_vector.scale.y = 0.05;
64 geometry_msgs::Point start;
65 start.x = info.nearest_point_obstacle_vector.x;
66 start.y = info.nearest_point_obstacle_vector.y;
67 start.z = info.nearest_point_obstacle_vector.z;
69 geometry_msgs::Point end;
70 end.x = info.nearest_point_frame_vector.x;
71 end.y = info.nearest_point_frame_vector.y;
72 end.z = info.nearest_point_frame_vector.z;
74 marker_vector.color.a = 1.0;
75 marker_vector.color.g = 1.0;
77 marker_vector.points.push_back(start);
78 marker_vector.points.push_back(end);
79 marker_array.markers.push_back(marker_vector);
83 visualization_msgs::Marker marker_distance;
84 marker_distance.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
86 marker_distance.action = visualization_msgs::Marker::ADD;
87 marker_distance.ns =
"distances";
88 marker_distance.id = 2*i+1;
89 marker_distance.header.frame_id = info.header.frame_id;
90 std::stringstream sstream;
91 sstream << std::setprecision(3) << info.distance;
92 marker_distance.text = sstream.str();
94 marker_distance.scale.x = 0.1;
95 marker_distance.scale.y = 0.1;
96 marker_distance.scale.z = 0.1;
98 marker_distance.color.a = 1.0;
102 marker_distance.color.r = 0.0;
103 marker_distance.color.g = 0.0;
104 marker_distance.color.b = 0.0;
106 marker_distance.pose.position.x = info.nearest_point_frame_vector.x;
107 marker_distance.pose.position.y = info.nearest_point_frame_vector.y + 0.05;
108 marker_distance.pose.position.z = info.nearest_point_frame_vector.z;
110 marker_array.markers.push_back(marker_distance);
113 marker_pub_.
publish(marker_array);
117 int main(
int argc,
char** argv)
119 ros::init(argc, argv,
"debug_obstacle_distance_node");
124 ROS_ERROR(
"Failed to initialize DebugDistanceManager.");
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int main(int argc, char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher marker_pub_
ros::Subscriber obstacle_distances_sub_
void obstacleDistancesCallback(const cob_control_msgs::ObstacleDistances::ConstPtr &msg)