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

Simplify start_trajectory service (RFC-28) (#1245) 

* Simplify start_trajectory.

* Ran clang-format.

* Make corrections based on the review

* Make corrections based on review #2, remove obsolete functions

* Remove unnecessary local variables
master
mgladkova 2019-05-07 16:53:06 +02:00 committed by Alexander Belyaev
parent 63dce89f6e
commit 92a8b81a84
11 changed files with 88 additions and 381 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
cartographer_ros/cartographer_ros/CMakeLists.txt
View File

@ -47,17 +47,6 @@ install(TARGETS cartographer_offline_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
) )
google_binary(cartographer_start_trajectory
SRCS
start_trajectory_main.cc
)
install(TARGETS cartographer_start_trajectory
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
google_binary(cartographer_occupancy_grid_node google_binary(cartographer_occupancy_grid_node
SRCS SRCS
occupancy_grid_node_main.cc occupancy_grid_node_main.cc

152
cartographer_ros/cartographer_ros/node.cc
View File

@ -56,19 +56,6 @@ using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState; ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
namespace { namespace {
cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
cartographer_ros_msgs::SensorTopics topics;
topics.laser_scan_topic = kLaserScanTopic;
topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
topics.point_cloud2_topic = kPointCloud2Topic;
topics.imu_topic = kImuTopic;
topics.odometry_topic = kOdometryTopic;
topics.nav_sat_fix_topic = kNavSatFixTopic;
topics.landmark_topic = kLandmarkTopic;
return topics;
}
// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and // Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and
// calls 'handler' on the 'node' to handle messages. Returns the subscriber. // calls 'handler' on the 'node' to handle messages. Returns the subscriber.
template <typename MessageType> template <typename MessageType>
@ -342,24 +329,21 @@ void Node::PublishConstraintList(
} }
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds( Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
using SensorType = SensorId::SensorType; using SensorType = SensorId::SensorType;
std::set<SensorId> expected_topics; std::set<SensorId> expected_topics;
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics. // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames( for (const std::string& topic : ComputeRepeatedTopicNames(
topics.laser_scan_topic, options.num_laser_scans)) { kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic}); expected_topics.insert(SensorId{SensorType::RANGE, topic});
} }
for (const std::string& topic : for (const std::string& topic :
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
options.num_multi_echo_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic}); expected_topics.insert(SensorId{SensorType::RANGE, topic});
} }
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
@ -368,17 +352,16 @@ Node::ComputeExpectedSensorIds(
(node_options_.map_builder_options.use_trajectory_builder_2d() && (node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options() options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) { .use_imu_data())) {
expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic}); expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
} }
// Odometry is optional. // Odometry is optional.
if (options.use_odometry) { if (options.use_odometry) {
expected_topics.insert( expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
SensorId{SensorType::ODOMETRY, topics.odometry_topic});
} }
// NavSatFix is optional. // NavSatFix is optional.
if (options.use_nav_sat) { if (options.use_nav_sat) {
expected_topics.insert( expected_topics.insert(
SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic}); SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
} }
// Landmark is optional. // Landmark is optional.
if (options.use_landmarks) { if (options.use_landmarks) {
@ -387,15 +370,14 @@ Node::ComputeExpectedSensorIds(
return expected_topics; return expected_topics;
} }
int Node::AddTrajectory(const TrajectoryOptions& options, int Node::AddTrajectory(const TrajectoryOptions& options) {
const cartographer_ros_msgs::SensorTopics& topics) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options, topics); expected_sensor_ids = ComputeExpectedSensorIds(options);
const int trajectory_id = const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options); AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options); AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, topics, trajectory_id); LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer( wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec), ::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
@ -406,27 +388,25 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
} }
void Node::LaunchSubscribers(const TrajectoryOptions& options, void Node::LaunchSubscribers(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics,
const int trajectory_id) { const int trajectory_id) {
for (const std::string& topic : ComputeRepeatedTopicNames( for (const std::string& topic :
topics.laser_scan_topic, options.num_laser_scans)) { ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>( {SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this), this),
topic}); topic});
} }
for (const std::string& topic : for (const std::string& topic : ComputeRepeatedTopicNames(
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>( {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this), &node_handle_, this),
topic}); topic});
} }
for (const std::string& topic : ComputeRepeatedTopicNames( for (const std::string& topic :
topics.point_cloud2_topic, options.num_point_clouds)) { ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::PointCloud2>( {SubscribeWithHandler<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic, &Node::HandlePointCloud2Message, trajectory_id, topic,
@ -440,37 +420,33 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
(node_options_.map_builder_options.use_trajectory_builder_2d() && (node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options() options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) { .use_imu_data())) {
std::string topic = topics.imu_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage, {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, topic, trajectory_id, kImuTopic,
&node_handle_, this), &node_handle_, this),
topic}); kImuTopic});
} }
if (options.use_odometry) { if (options.use_odometry) {
std::string topic = topics.odometry_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage, {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, topic, trajectory_id, kOdometryTopic,
&node_handle_, this), &node_handle_, this),
topic}); kOdometryTopic});
} }
if (options.use_nav_sat) { if (options.use_nav_sat) {
std::string topic = topics.nav_sat_fix_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>( {SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_, &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
this), &node_handle_, this),
topic}); kNavSatFixTopic});
} }
if (options.use_landmarks) { if (options.use_landmarks) {
std::string topic = topics.landmark_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>( {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_, &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
this), &node_handle_, this),
topic}); kLandmarkTopic});
} }
} }
@ -486,10 +462,8 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
return false; return false;
} }
bool Node::ValidateTopicNames( bool Node::ValidateTopicNames(const TrajectoryOptions& options) {
const ::cartographer_ros_msgs::SensorTopics& topics, for (const auto& sensor_id : ComputeExpectedSensorIds(options)) {
const TrajectoryOptions& options) {
for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) {
const std::string& topic = sensor_id.id; const std::string& topic = sensor_id.id;
if (subscribed_topics_.count(topic) > 0) { if (subscribed_topics_.count(topic) > 0) {
LOG(ERROR) << "Topic name [" << topic << "] is already used."; LOG(ERROR) << "Topic name [" << topic << "] is already used.";
@ -562,23 +536,56 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
bool Node::HandleStartTrajectory( bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) { ::cartographer_ros_msgs::StartTrajectory::Response& response) {
absl::MutexLock lock(&mutex_); TrajectoryOptions trajectory_options;
TrajectoryOptions options; std::tie(std::ignore, trajectory_options) = LoadOptions(
if (!FromRosMessage(request.options, &options) || request.configuration_directory, request.configuration_basename);
!ValidateTrajectoryOptions(options)) {
const std::string error = "Invalid trajectory options."; if (request.use_initial_pose) {
LOG(ERROR) << error; const auto pose = ToRigid3d(request.initial_pose);
if (!pose.IsValid()) {
response.status.message =
"Invalid pose argument. Orientation quaternion must be normalized.";
LOG(ERROR) << response.status.message;
response.status.code =
cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
return true;
}
// Check if the requested trajectory for the relative initial pose exists.
response.status = TrajectoryStateToStatus(
request.relative_to_trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
TrajectoryState::FINISHED} /* valid states */);
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't start a trajectory with initial pose: "
<< response.status.message;
return true;
}
::cartographer::mapping::proto::InitialTrajectoryPose
initial_trajectory_pose;
initial_trajectory_pose.set_to_trajectory_id(
request.relative_to_trajectory_id);
*initial_trajectory_pose.mutable_relative_pose() =
cartographer::transform::ToProto(pose);
initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal(
::cartographer_ros::FromRos(ros::Time(0))));
*trajectory_options.trajectory_builder_options
.mutable_initial_trajectory_pose() = initial_trajectory_pose;
}
if (!ValidateTrajectoryOptions(trajectory_options)) {
response.status.message = "Invalid trajectory options.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
response.status.message = error; } else if (!ValidateTopicNames(trajectory_options)) {
} else if (!ValidateTopicNames(request.topics, options)) { response.status.message = "Topics are already used by another trajectory.";
const std::string error = "Invalid topics."; LOG(ERROR) << response.status.message;
LOG(ERROR) << error;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
response.status.message = error;
} else { } else {
response.trajectory_id = AddTrajectory(options, request.topics);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message = "Success."; response.status.message = "Success.";
response.trajectory_id = AddTrajectory(trajectory_options);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
} }
return true; return true;
} }
@ -586,7 +593,7 @@ bool Node::HandleStartTrajectory(
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_); absl::MutexLock lock(&mutex_);
CHECK(ValidateTrajectoryOptions(options)); CHECK(ValidateTrajectoryOptions(options));
AddTrajectory(options, DefaultSensorTopics()); AddTrajectory(options);
} }
std::vector< std::vector<
@ -601,8 +608,7 @@ Node::ComputeDefaultSensorIdsForMultipleBags(
prefix = "bag_" + std::to_string(i + 1) + "_"; prefix = "bag_" + std::to_string(i + 1) + "_";
} }
std::set<SensorId> unique_sensor_ids; std::set<SensorId> unique_sensor_ids;
for (const auto& sensor_id : for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) {
ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) {
unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id});
} }
bags_sensor_ids.push_back(unique_sensor_ids); bags_sensor_ids.push_back(unique_sensor_ids);

16
cartographer_ros/cartographer_ros/node.h
View File

@ -36,13 +36,11 @@
#include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/FinishTrajectory.h"
#include "cartographer_ros_msgs/GetTrajectoryStates.h" #include "cartographer_ros_msgs/GetTrajectoryStates.h"
#include "cartographer_ros_msgs/ReadMetrics.h" #include "cartographer_ros_msgs/ReadMetrics.h"
#include "cartographer_ros_msgs/SensorTopics.h"
#include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/StatusResponse.h" #include "cartographer_ros_msgs/StatusResponse.h"
#include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/SubmapQuery.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "cartographer_ros_msgs/WriteState.h" #include "cartographer_ros_msgs/WriteState.h"
#include "nav_msgs/Odometry.h" #include "nav_msgs/Odometry.h"
#include "ros/ros.h" #include "ros/ros.h"
@ -155,14 +153,9 @@ class Node {
// Returns the set of SensorIds expected for a trajectory. // Returns the set of SensorIds expected for a trajectory.
// 'SensorId::id' is the expected ROS topic name. // 'SensorId::id' is the expected ROS topic name.
std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
ComputeExpectedSensorIds( ComputeExpectedSensorIds(const TrajectoryOptions& options) const;
const TrajectoryOptions& options, int AddTrajectory(const TrajectoryOptions& options);
const cartographer_ros_msgs::SensorTopics& topics) const; void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id);
int AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics);
void LaunchSubscribers(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics,
int trajectory_id);
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
@ -171,8 +164,7 @@ class Node {
void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event); void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
bool ValidateTrajectoryOptions(const TrajectoryOptions& options); bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, bool ValidateTopicNames(const TrajectoryOptions& options);
const TrajectoryOptions& options);
cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);

1
cartographer_ros/cartographer_ros/node_options.h
View File

@ -44,7 +44,6 @@ NodeOptions CreateNodeOptions(
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions( std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const std::string& configuration_directory, const std::string& configuration_directory,
const std::string& configuration_basename); const std::string& configuration_basename);
} // namespace cartographer_ros } // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H

126
cartographer_ros/cartographer_ros/start_trajectory_main.cc
View File

@ -1,126 +0,0 @@
/*
* Copyright 2016 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <string>
#include <vector>
#include "absl/memory/memory.h"
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/node_constants.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "gflags/gflags.h"
#include "ros/ros.h"
DEFINE_string(configuration_directory, "",
"First directory in which configuration files are searched, "
"second is always the Cartographer installation to allow "
"including files from there.");
DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(initial_pose, "", "Starting pose of a new trajectory");
namespace cartographer_ros {
namespace {
TrajectoryOptions LoadOptions() {
auto file_resolver =
absl::make_unique<cartographer::common::ConfigurationFileResolver>(
std::vector<std::string>{FLAGS_configuration_directory});
const std::string code =
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
auto lua_parameter_dictionary =
cartographer::common::LuaParameterDictionary::NonReferenceCounted(
code, std::move(file_resolver));
if (!FLAGS_initial_pose.empty()) {
auto initial_trajectory_pose_file_resolver =
absl::make_unique<cartographer::common::ConfigurationFileResolver>(
std::vector<std::string>{FLAGS_configuration_directory});
auto initial_trajectory_pose =
cartographer::common::LuaParameterDictionary::NonReferenceCounted(
"return " + FLAGS_initial_pose,
std::move(initial_trajectory_pose_file_resolver));
return CreateTrajectoryOptions(lua_parameter_dictionary.get(),
initial_trajectory_pose.get());
} else {
return CreateTrajectoryOptions(lua_parameter_dictionary.get());
}
}
bool Run() {
ros::NodeHandle node_handle;
ros::ServiceClient client =
node_handle.serviceClient<cartographer_ros_msgs::StartTrajectory>(
kStartTrajectoryServiceName);
cartographer_ros_msgs::StartTrajectory srv;
srv.request.options = ToRosMessage(LoadOptions());
srv.request.topics.laser_scan_topic = node_handle.resolveName(
kLaserScanTopic, true /* apply topic remapping */);
srv.request.topics.multi_echo_laser_scan_topic =
node_handle.resolveName(kMultiEchoLaserScanTopic, true);
srv.request.topics.point_cloud2_topic =
node_handle.resolveName(kPointCloud2Topic, true);
srv.request.topics.imu_topic = node_handle.resolveName(kImuTopic, true);
srv.request.topics.odometry_topic =
node_handle.resolveName(kOdometryTopic, true);
if (!client.call(srv)) {
LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << ".";
return false;
}
if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Error starting trajectory - message: '"
<< srv.response.status.message
<< "' (status code: " << std::to_string(srv.response.status.code)
<< ").";
return false;
}
LOG(INFO) << "Started trajectory " << srv.response.trajectory_id;
return true;
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::SetUsageMessage(
"\n\n"
"Convenience tool around the start_trajectory service. This takes a Lua "
"file that is accepted by the node as well and starts a new trajectory "
"using its settings.\n");
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
::ros::init(argc, argv, "cartographer_start_trajectory");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
int exit_code = cartographer_ros::Run() ? 0 : 1;
::ros::shutdown();
return exit_code;
}

81
cartographer_ros/cartographer_ros/trajectory_options.cc
View File

@ -79,85 +79,4 @@ TrajectoryOptions CreateTrajectoryOptions(
CheckTrajectoryOptions(options); CheckTrajectoryOptions(options);
return options; return options;
} }
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary,
::cartographer::common::LuaParameterDictionary* initial_trajectory_pose) {
TrajectoryOptions options = CreateTrajectoryOptions(lua_parameter_dictionary);
*options.trajectory_builder_options.mutable_initial_trajectory_pose() =
CreateInitialTrajectoryPose(initial_trajectory_pose);
return options;
}
::cartographer::mapping::proto::InitialTrajectoryPose
CreateInitialTrajectoryPose(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary) {
::cartographer::mapping::proto::InitialTrajectoryPose pose;
pose.set_to_trajectory_id(
lua_parameter_dictionary->GetNonNegativeInt("to_trajectory_id"));
*pose.mutable_relative_pose() =
cartographer::transform::ToProto(cartographer::transform::FromDictionary(
lua_parameter_dictionary->GetDictionary("relative_pose").get()));
pose.set_timestamp(
lua_parameter_dictionary->HasKey("timestamp")
? lua_parameter_dictionary->GetNonNegativeInt("timestamp")
: cartographer::common::ToUniversal(FromRos(ros::Time::now())));
return pose;
}
bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
TrajectoryOptions* options) {
options->tracking_frame = msg.tracking_frame;
options->published_frame = msg.published_frame;
options->odom_frame = msg.odom_frame;
options->provide_odom_frame = msg.provide_odom_frame;
options->use_odometry = msg.use_odometry;
options->use_nav_sat = msg.use_nav_sat;
options->use_landmarks = msg.use_landmarks;
options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d;
options->num_laser_scans = msg.num_laser_scans;
options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
options->num_subdivisions_per_laser_scan =
msg.num_subdivisions_per_laser_scan;
options->num_point_clouds = msg.num_point_clouds;
options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio;
options->odometry_sampling_ratio = msg.odometry_sampling_ratio;
options->fixed_frame_pose_sampling_ratio =
msg.fixed_frame_pose_sampling_ratio;
options->imu_sampling_ratio = msg.imu_sampling_ratio;
options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio;
if (!options->trajectory_builder_options.ParseFromString(
msg.trajectory_builder_options_proto)) {
LOG(ERROR) << "Failed to parse protobuf";
return false;
}
CheckTrajectoryOptions(*options);
return true;
}
cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
const TrajectoryOptions& options) {
cartographer_ros_msgs::TrajectoryOptions msg;
msg.tracking_frame = options.tracking_frame;
msg.published_frame = options.published_frame;
msg.odom_frame = options.odom_frame;
msg.provide_odom_frame = options.provide_odom_frame;
msg.use_odometry = options.use_odometry;
msg.use_nav_sat = options.use_nav_sat;
msg.use_landmarks = options.use_landmarks;
msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d;
msg.num_laser_scans = options.num_laser_scans;
msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
msg.num_point_clouds = options.num_point_clouds;
msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio;
msg.odometry_sampling_ratio = options.odometry_sampling_ratio;
msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio;
msg.imu_sampling_ratio = options.imu_sampling_ratio;
msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio;
options.trajectory_builder_options.SerializeToString(
&msg.trajectory_builder_options_proto);
return msg;
}
} // namespace cartographer_ros } // namespace cartographer_ros

17
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -22,7 +22,6 @@
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -48,25 +47,9 @@ struct TrajectoryOptions {
double landmarks_sampling_ratio; double landmarks_sampling_ratio;
}; };
::cartographer::mapping::proto::InitialTrajectoryPose
CreateInitialTrajectoryPose(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
TrajectoryOptions CreateTrajectoryOptions( TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
TrajectoryOptions CreateTrajectoryOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary,
::cartographer::common::LuaParameterDictionary* initial_trajectory_pose);
// Try to convert 'msg' into 'options'. Returns false on failure.
bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
TrajectoryOptions* options);
// Converts 'trajectory_options' into a ROS message.
cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
const TrajectoryOptions& trajectory_options);
} // namespace cartographer_ros } // namespace cartographer_ros
#endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H

2
cartographer_ros_msgs/CMakeLists.txt
View File

@ -33,13 +33,11 @@ add_message_files(
MetricFamily.msg MetricFamily.msg
MetricLabel.msg MetricLabel.msg
Metric.msg Metric.msg
SensorTopics.msg
StatusCode.msg StatusCode.msg
StatusResponse.msg StatusResponse.msg
SubmapEntry.msg SubmapEntry.msg
SubmapList.msg SubmapList.msg
SubmapTexture.msg SubmapTexture.msg
TrajectoryOptions.msg
TrajectoryStates.msg TrajectoryStates.msg
) )

21
cartographer_ros_msgs/msg/SensorTopics.msg
View File

@ -1,21 +0,0 @@
# Copyright 2016 The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
string laser_scan_topic
string multi_echo_laser_scan_topic
string point_cloud2_topic
string imu_topic
string odometry_topic
string nav_sat_fix_topic
string landmark_topic

35
cartographer_ros_msgs/msg/TrajectoryOptions.msg
View File

@ -1,35 +0,0 @@
# Copyright 2016 The Cartographer Authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
string tracking_frame
string published_frame
string odom_frame
bool provide_odom_frame
bool use_odometry
bool use_nav_sat
bool use_landmarks
bool publish_frame_projected_to_2d
int32 num_laser_scans
int32 num_multi_echo_laser_scans
int32 num_subdivisions_per_laser_scan
int32 num_point_clouds
float64 rangefinder_sampling_ratio
float64 odometry_sampling_ratio
float64 fixed_frame_pose_sampling_ratio
float64 imu_sampling_ratio
float64 landmarks_sampling_ratio
# This is a binary-encoded
# 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto.
string trajectory_builder_options_proto

7
cartographer_ros_msgs/srv/StartTrajectory.srv
View File

@ -12,8 +12,11 @@
# See the License for the specific language governing permissions and # See the License for the specific language governing permissions and
# limitations under the License. # limitations under the License.
cartographer_ros_msgs/TrajectoryOptions options string configuration_directory
cartographer_ros_msgs/SensorTopics topics string configuration_basename
bool use_initial_pose
geometry_msgs/Pose initial_pose
int32 relative_to_trajectory_id
--- ---
cartographer_ros_msgs/StatusResponse status cartographer_ros_msgs/StatusResponse status
int32 trajectory_id int32 trajectory_id