12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cartographer_ros/cartographer_ros/assets_writer.cc
View File

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

10
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

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

2
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

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

14
cartographer_ros/cartographer_ros/node.cc
View File

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

4
cartographer_ros/cartographer_ros/node.h
View File

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

2
cartographer_ros/cartographer_ros/node_constants.h
View File

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

2
cartographer_ros/configuration_files/demo_2d.rviz
View File

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

2
cartographer_ros/configuration_files/demo_3d.rviz
View File

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