RViz settings for landmarks. (#717)
[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)master
parent
c2b0143a5e
commit
7b9e2fe2e1
|
@ -30,6 +30,7 @@ namespace {
|
||||||
using ::cartographer::transform::Rigid3d;
|
using ::cartographer::transform::Rigid3d;
|
||||||
|
|
||||||
constexpr double kTrajectoryLineStripMarkerScale = 0.07;
|
constexpr double kTrajectoryLineStripMarkerScale = 0.07;
|
||||||
|
constexpr double kLandmarkMarkerScale = 0.3;
|
||||||
constexpr double kConstraintMarkerScale = 0.025;
|
constexpr double kConstraintMarkerScale = 0.025;
|
||||||
|
|
||||||
::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
|
::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
|
||||||
|
@ -62,7 +63,7 @@ int GetLandmarkIndex(
|
||||||
auto it = landmark_id_to_index->find(landmark_id);
|
auto it = landmark_id_to_index->find(landmark_id);
|
||||||
if (it == landmark_id_to_index->end()) {
|
if (it == landmark_id_to_index->end()) {
|
||||||
const int new_index = landmark_id_to_index->size();
|
const int new_index = landmark_id_to_index->size();
|
||||||
landmark_id_to_index->at(landmark_id) = new_index;
|
landmark_id_to_index->emplace(landmark_id, new_index);
|
||||||
return new_index;
|
return new_index;
|
||||||
}
|
}
|
||||||
return it->second;
|
return it->second;
|
||||||
|
@ -74,9 +75,12 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
|
||||||
visualization_msgs::Marker marker;
|
visualization_msgs::Marker marker;
|
||||||
marker.ns = "Landmarks";
|
marker.ns = "Landmarks";
|
||||||
marker.id = landmark_index;
|
marker.id = landmark_index;
|
||||||
marker.type = visualization_msgs::Marker::SPHERE;
|
marker.type = visualization_msgs::Marker::CUBE;
|
||||||
marker.header.stamp = ::ros::Time::now();
|
marker.header.stamp = ::ros::Time::now();
|
||||||
marker.header.frame_id = frame_id;
|
marker.header.frame_id = frame_id;
|
||||||
|
marker.scale.x = kLandmarkMarkerScale;
|
||||||
|
marker.scale.y = kLandmarkMarkerScale;
|
||||||
|
marker.scale.z = kLandmarkMarkerScale;
|
||||||
marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
|
marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
|
||||||
marker.pose = ToGeometryMsgPose(landmark_pose);
|
marker.pose = ToGeometryMsgPose(landmark_pose);
|
||||||
return marker;
|
return marker;
|
||||||
|
|
|
@ -177,6 +177,14 @@ Visualization Manager:
|
||||||
"": true
|
"": true
|
||||||
Queue Size: 100
|
Queue Size: 100
|
||||||
Value: true
|
Value: true
|
||||||
|
- Class: rviz/MarkerArray
|
||||||
|
Enabled: true
|
||||||
|
Marker Topic: /landmark_poses_list
|
||||||
|
Name: Landmark Poses
|
||||||
|
Namespaces:
|
||||||
|
"": true
|
||||||
|
Queue Size: 100
|
||||||
|
Value: true
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 100; 100; 100
|
Background Color: 100; 100; 100
|
||||||
|
|
|
@ -239,6 +239,14 @@ Visualization Manager:
|
||||||
"": true
|
"": true
|
||||||
Queue Size: 100
|
Queue Size: 100
|
||||||
Value: true
|
Value: true
|
||||||
|
- Class: rviz/MarkerArray
|
||||||
|
Enabled: true
|
||||||
|
Marker Topic: /landmark_poses_list
|
||||||
|
Name: Landmark Poses
|
||||||
|
Namespaces:
|
||||||
|
"": true
|
||||||
|
Queue Size: 100
|
||||||
|
Value: true
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 100; 100; 100
|
Background Color: 100; 100; 100
|
||||||
|
|
Loading…
Reference in New Issue