21 #include <visualization_msgs/MarkerArray.h> 22 #include "cob_control_msgs/ObstacleDistance.h" 23 #include "cob_control_msgs/ObstacleDistances.h" 36 if (!nh_.
getParam(
"chain_base_link", this->chain_base_link_))
38 ROS_ERROR(
"Failed to get parameter \"chain_base_link\".");
42 marker_pub_ = this->nh_.
advertise<visualization_msgs::MarkerArray>(
"obstacle_distance/distance_markers", 1,
true);
51 visualization_msgs::MarkerArray marker_array;
52 std::map<std::string, cob_control_msgs::ObstacleDistance> relevant_obstacle_distances;
54 for (uint32_t i = 0; i < msg->distances.size(); i++)
56 const std::string
id = msg->distances[i].link_of_interest;
57 if (relevant_obstacle_distances.count(
id) > 0)
59 if (relevant_obstacle_distances[
id].
distance > msg->distances[i].distance)
61 relevant_obstacle_distances[
id] = msg->distances[i];
66 relevant_obstacle_distances[
id] = msg->distances[i];
70 for (std::map<std::string, cob_control_msgs::ObstacleDistance>::const_iterator it = relevant_obstacle_distances.begin();
71 it != relevant_obstacle_distances.end(); ++it)
74 visualization_msgs::Marker marker_vector;
75 marker_vector.type = visualization_msgs::Marker::ARROW;
77 marker_vector.action = visualization_msgs::Marker::ADD;
78 marker_vector.ns = it->first;
79 marker_vector.id = 42;
82 marker_vector.scale.x = 0.01;
83 marker_vector.scale.y = 0.05;
85 geometry_msgs::Point start;
86 start.x = it->second.nearest_point_obstacle_vector.x;
87 start.y = it->second.nearest_point_obstacle_vector.y;
88 start.z = it->second.nearest_point_obstacle_vector.z;
90 geometry_msgs::Point end;
91 end.x = it->second.nearest_point_frame_vector.x;
92 end.y = it->second.nearest_point_frame_vector.y;
93 end.z = it->second.nearest_point_frame_vector.z;
95 marker_vector.color.a = 1.0;
96 marker_vector.color.g = 1.0;
98 marker_vector.points.push_back(start);
99 marker_vector.points.push_back(end);
100 marker_array.markers.push_back(marker_vector);
104 visualization_msgs::Marker marker_distance;
105 marker_distance.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
107 marker_distance.action = visualization_msgs::Marker::ADD;
108 marker_distance.ns = it->first;
109 marker_distance.id = 69;
111 std::stringstream sstream;
112 sstream << std::setprecision(3) << it->second.distance;
113 marker_distance.text = sstream.str();
115 marker_distance.scale.x = 0.1;
116 marker_distance.scale.y = 0.1;
117 marker_distance.scale.z = 0.1;
119 marker_distance.color.a = 1.0;
123 marker_distance.color.r = 0.0;
124 marker_distance.color.g = 0.0;
125 marker_distance.color.b = 0.0;
127 marker_distance.pose.position.x = it->second.nearest_point_frame_vector.x;
128 marker_distance.pose.position.y = it->second.nearest_point_frame_vector.y + 0.05;
129 marker_distance.pose.position.z = it->second.nearest_point_frame_vector.z;
131 marker_array.markers.push_back(marker_distance);
134 this->marker_pub_.
publish(marker_array);
140 int main(
int argc,
char** argv)
142 ros::init(argc, argv,
"debug_obstacle_distance_node");
147 ROS_ERROR(
"Failed to initialize DebugDistanceManager.");
int main(int argc, char **argv)
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)
std::string chain_base_link_
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
ROSCPP_DECL void spin(Spinner &spinner)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool getParam(const std::string &key, std::string &s) const
ros::Publisher marker_pub_
ros::Subscriber obstacle_distances_sub_
void obstacleDistancesCallback(const cob_control_msgs::ObstacleDistances::ConstPtr &msg)