Rename "nodes list" to "node list". (#402)

master
Juraj Oršulić 2017-07-03 10:42:44 +02:00 committed by Wolfgang Hess
parent 1e76e8f4ae
commit 7c31232208
8 changed files with 24 additions and 27 deletions

View File

@ -67,19 +67,16 @@ void Write3DAssets(
carto::io::NullPointsProcessor null_points_processor;
carto::io::XRayPointsProcessor xy_xray_points_processor(
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
{}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
carto::io::XRayPointsProcessor yz_xray_points_processor(
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
carto::io::XRayPointsProcessor xz_xray_points_processor(
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
file_writer_factory(stem + ".ply"), &xz_xray_points_processor);

View File

@ -203,8 +203,8 @@ MapBuilderBridge::GetTrajectoryStates() {
return trajectory_states;
}
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
visualization_msgs::MarkerArray trajectory_nodes_list;
visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
visualization_msgs::MarkerArray trajectory_node_list;
const auto all_trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
int marker_id = 0;
@ -230,16 +230,16 @@ visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodesList() {
// Work around the 16384 point limit in rviz by splitting the
// trajectory into multiple markers.
if (marker.points.size() == 16384) {
trajectory_nodes_list.markers.push_back(marker);
trajectory_node_list.markers.push_back(marker);
marker.id = marker_id++;
marker.points.clear();
// Push back the last point, so the two markers appear connected.
marker.points.push_back(node_point);
}
}
trajectory_nodes_list.markers.push_back(marker);
trajectory_node_list.markers.push_back(marker);
}
return trajectory_nodes_list;
return trajectory_node_list;
}
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {

View File

@ -62,7 +62,7 @@ class MapBuilderBridge {
cartographer_ros_msgs::SubmapList GetSubmapList();
std::unique_ptr<nav_msgs::OccupancyGrid> BuildOccupancyGrid();
std::unordered_map<int, TrajectoryState> GetTrajectoryStates();
visualization_msgs::MarkerArray GetTrajectoryNodesList();
visualization_msgs::MarkerArray GetTrajectoryNodeList();
SensorBridge* sensor_bridge(int trajectory_id);

View File

@ -85,9 +85,9 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
trajectory_nodes_list_publisher_ =
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodesListTopic, kLatestOnlyPublisherQueueSize);
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
@ -118,7 +118,7 @@ Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
&Node::PublishTrajectoryStates, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodesList, this));
&Node::PublishTrajectoryNodeList, this));
}
Node::~Node() {
@ -208,12 +208,12 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
}
}
void Node::PublishTrajectoryNodesList(
void Node::PublishTrajectoryNodeList(
const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_);
if (trajectory_nodes_list_publisher_.getNumSubscribers() > 0) {
trajectory_nodes_list_publisher_.publish(
map_builder_bridge_.GetTrajectoryNodesList());
if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
trajectory_node_list_publisher_.publish(
map_builder_bridge_.GetTrajectoryNodeList());
}
}

View File

@ -76,7 +76,7 @@ class Node {
int trajectory_id);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryNodesList(const ::ros::WallTimerEvent& timer_event);
void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
void SpinOccupancyGridThreadForever();
bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics,
@ -91,7 +91,7 @@ class Node {
::ros::NodeHandle node_handle_;
::ros::Publisher submap_list_publisher_;
::ros::Publisher trajectory_nodes_list_publisher_;
::ros::Publisher trajectory_node_list_publisher_;
// These ros::ServiceServers need to live for the lifetime of the node.
std::vector<::ros::ServiceServer> service_servers_;
::ros::Publisher scan_matched_point_cloud_publisher_;

View File

@ -32,7 +32,7 @@ constexpr char kSubmapListTopic[] = "submap_list";
constexpr char kSubmapQueryServiceName[] = "submap_query";
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
constexpr char kWriteAssetsServiceName[] = "write_assets";
constexpr char kTrajectoryNodesListTopic[] = "trajectory_nodes_list";
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
} // namespace cartographer_ros

View File

@ -172,7 +172,7 @@ Visualization Manager:
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /trajectory_nodes_list
Marker Topic: /trajectory_node_list
Name: MarkerArray
Namespaces:
"": true

View File

@ -234,7 +234,7 @@ Visualization Manager:
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /trajectory_nodes_list
Marker Topic: /trajectory_node_list
Name: MarkerArray
Namespaces:
"": true