* \brief Create marker to be visualized in RViz * * \param[in] id The marker ID. * \param[in] frame_id The marker reference frame. * \param[in] position The marker position. * \param[in] color The marker color. */
| 137 | * \param[in] color The marker color. |
| 138 | */ |
| 139 | visualization_msgs::Marker |
| 140 | createMarker (int id, std::string frame_id, ros::Time stamp, Eigen::Vector3d position, cv::Vec3f color) |
| 141 | { |
| 142 | visualization_msgs::Marker marker; |
| 143 | |
| 144 | marker.header.frame_id = world_frame_id; |
| 145 | marker.header.stamp = stamp; |
| 146 | marker.ns = frame_id; |
| 147 | marker.id = id; |
| 148 | marker.type = visualization_msgs::Marker::SPHERE; |
| 149 | marker.action = visualization_msgs::Marker::ADD; |
| 150 | marker.pose.position.x = position(0); |
| 151 | marker.pose.position.y = position(1); |
| 152 | marker.pose.position.z = position(2); |
| 153 | marker.scale.x = 0.1; |
| 154 | marker.scale.y = 0.1; |
| 155 | marker.scale.z = 0.1; |
| 156 | marker.color.r = color(0); |
| 157 | marker.color.g = color(1); |
| 158 | marker.color.b = color(2); |
| 159 | marker.color.a = 1.0; |
| 160 | marker.lifetime = ros::Duration(0.2); |
| 161 | |
| 162 | return marker; |
| 163 | } |
| 164 | |
| 165 | void |
| 166 | plotCameraLegend (std::map<std::string, int> curr_color_map) |
no outgoing calls
no test coverage detected