From 92a8b81a8489dd71a73c6d838c560712311b1da5 Mon Sep 17 00:00:00 2001 From: mgladkova Date: Tue, 7 May 2019 16:53:06 +0200 Subject: [PATCH] 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 --- .../cartographer_ros/CMakeLists.txt | 11 -- cartographer_ros/cartographer_ros/node.cc | 152 +++++++++--------- cartographer_ros/cartographer_ros/node.h | 16 +- .../cartographer_ros/node_options.h | 1 - .../cartographer_ros/start_trajectory_main.cc | 126 --------------- .../cartographer_ros/trajectory_options.cc | 81 ---------- .../cartographer_ros/trajectory_options.h | 17 -- cartographer_ros_msgs/CMakeLists.txt | 2 - cartographer_ros_msgs/msg/SensorTopics.msg | 21 --- .../msg/TrajectoryOptions.msg | 35 ---- cartographer_ros_msgs/srv/StartTrajectory.srv | 7 +- 11 files changed, 88 insertions(+), 381 deletions(-) delete mode 100644 cartographer_ros/cartographer_ros/start_trajectory_main.cc delete mode 100644 cartographer_ros_msgs/msg/SensorTopics.msg delete mode 100644 cartographer_ros_msgs/msg/TrajectoryOptions.msg diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index 667748d..f2616d1 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -47,17 +47,6 @@ install(TARGETS cartographer_offline_node 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 SRCS occupancy_grid_node_main.cc diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 6ed5dad..1bc804a 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -56,19 +56,6 @@ using TrajectoryState = ::cartographer::mapping::PoseGraphInterface::TrajectoryState; 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 // calls 'handler' on the 'node' to handle messages. Returns the subscriber. template @@ -342,24 +329,21 @@ void Node::PublishConstraintList( } std::set -Node::ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) const { +Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const { using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId; using SensorType = SensorId::SensorType; std::set expected_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( - topics.laser_scan_topic, options.num_laser_scans)) { + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } for (const std::string& topic : - ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, - 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)) { + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { expected_topics.insert(SensorId{SensorType::RANGE, topic}); } // 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() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic}); + expected_topics.insert(SensorId{SensorType::IMU, kImuTopic}); } // Odometry is optional. if (options.use_odometry) { - expected_topics.insert( - SensorId{SensorType::ODOMETRY, topics.odometry_topic}); + expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic}); } // NavSatFix is optional. if (options.use_nav_sat) { expected_topics.insert( - SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic}); + SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic}); } // Landmark is optional. if (options.use_landmarks) { @@ -387,15 +370,14 @@ Node::ComputeExpectedSensorIds( return expected_topics; } -int Node::AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) { +int Node::AddTrajectory(const TrajectoryOptions& options) { const std::set - expected_sensor_ids = ComputeExpectedSensorIds(options, topics); + expected_sensor_ids = ComputeExpectedSensorIds(options); const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); AddSensorSamplers(trajectory_id, options); - LaunchSubscribers(options, topics, trajectory_id); + LaunchSubscribers(options, trajectory_id); wall_timers_.push_back(node_handle_.createWallTimer( ::ros::WallDuration(kTopicMismatchCheckDelaySec), &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true)); @@ -406,27 +388,25 @@ int Node::AddTrajectory(const TrajectoryOptions& options, } void Node::LaunchSubscribers(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics, const int trajectory_id) { - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.laser_scan_topic, options.num_laser_scans)) { + for (const std::string& topic : + ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, this), topic}); } - for (const std::string& topic : - ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, - options.num_multi_echo_laser_scans)) { + for (const std::string& topic : ComputeRepeatedTopicNames( + kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, &node_handle_, this), topic}); } - for (const std::string& topic : ComputeRepeatedTopicNames( - topics.point_cloud2_topic, options.num_point_clouds)) { + for (const std::string& topic : + ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( &Node::HandlePointCloud2Message, trajectory_id, topic, @@ -440,37 +420,33 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - std::string topic = topics.imu_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler(&Node::HandleImuMessage, - trajectory_id, topic, + trajectory_id, kImuTopic, &node_handle_, this), - topic}); + kImuTopic}); } if (options.use_odometry) { - std::string topic = topics.odometry_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler(&Node::HandleOdometryMessage, - trajectory_id, topic, + trajectory_id, kOdometryTopic, &node_handle_, this), - topic}); + kOdometryTopic}); } if (options.use_nav_sat) { - std::string topic = topics.nav_sat_fix_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler( - &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_, - this), - topic}); + &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic, + &node_handle_, this), + kNavSatFixTopic}); } if (options.use_landmarks) { - std::string topic = topics.landmark_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler( - &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_, - this), - topic}); + &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic, + &node_handle_, this), + kLandmarkTopic}); } } @@ -486,10 +462,8 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { return false; } -bool Node::ValidateTopicNames( - const ::cartographer_ros_msgs::SensorTopics& topics, - const TrajectoryOptions& options) { - for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) { +bool Node::ValidateTopicNames(const TrajectoryOptions& options) { + for (const auto& sensor_id : ComputeExpectedSensorIds(options)) { const std::string& topic = sensor_id.id; if (subscribed_topics_.count(topic) > 0) { LOG(ERROR) << "Topic name [" << topic << "] is already used."; @@ -562,23 +536,56 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( bool Node::HandleStartTrajectory( ::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Response& response) { - absl::MutexLock lock(&mutex_); - TrajectoryOptions options; - if (!FromRosMessage(request.options, &options) || - !ValidateTrajectoryOptions(options)) { - const std::string error = "Invalid trajectory options."; - LOG(ERROR) << error; + TrajectoryOptions trajectory_options; + std::tie(std::ignore, trajectory_options) = LoadOptions( + request.configuration_directory, request.configuration_basename); + + if (request.use_initial_pose) { + 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.message = error; - } else if (!ValidateTopicNames(request.topics, options)) { - const std::string error = "Invalid topics."; - LOG(ERROR) << error; + } else if (!ValidateTopicNames(trajectory_options)) { + response.status.message = "Topics are already used by another trajectory."; + LOG(ERROR) << response.status.message; response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - response.status.message = error; } else { - response.trajectory_id = AddTrajectory(options, request.topics); - response.status.code = cartographer_ros_msgs::StatusCode::OK; response.status.message = "Success."; + response.trajectory_id = AddTrajectory(trajectory_options); + response.status.code = cartographer_ros_msgs::StatusCode::OK; } return true; } @@ -586,7 +593,7 @@ bool Node::HandleStartTrajectory( void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { absl::MutexLock lock(&mutex_); CHECK(ValidateTrajectoryOptions(options)); - AddTrajectory(options, DefaultSensorTopics()); + AddTrajectory(options); } std::vector< @@ -601,8 +608,7 @@ Node::ComputeDefaultSensorIdsForMultipleBags( prefix = "bag_" + std::to_string(i + 1) + "_"; } std::set unique_sensor_ids; - for (const auto& sensor_id : - ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) { + for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) { unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id}); } bags_sensor_ids.push_back(unique_sensor_ids); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 690487c..ac715d0 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -36,13 +36,11 @@ #include "cartographer_ros_msgs/FinishTrajectory.h" #include "cartographer_ros_msgs/GetTrajectoryStates.h" #include "cartographer_ros_msgs/ReadMetrics.h" -#include "cartographer_ros_msgs/SensorTopics.h" #include "cartographer_ros_msgs/StartTrajectory.h" #include "cartographer_ros_msgs/StatusResponse.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapQuery.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" #include "cartographer_ros_msgs/WriteState.h" #include "nav_msgs/Odometry.h" #include "ros/ros.h" @@ -155,14 +153,9 @@ class Node { // Returns the set of SensorIds expected for a trajectory. // 'SensorId::id' is the expected ROS topic name. std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId> - ComputeExpectedSensorIds( - const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) const; - 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); + ComputeExpectedSensorIds(const TrajectoryOptions& options) const; + int AddTrajectory(const TrajectoryOptions& options); + void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void AddExtrapolator(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 PublishConstraintList(const ::ros::WallTimerEvent& timer_event); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); - bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, - const TrajectoryOptions& options); + bool ValidateTopicNames(const TrajectoryOptions& options); cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_); void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 3021581..43dc219 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -44,7 +44,6 @@ NodeOptions CreateNodeOptions( std::tuple LoadOptions( const std::string& configuration_directory, const std::string& configuration_basename); - } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc deleted file mode 100644 index b0181d5..0000000 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ /dev/null @@ -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 -#include - -#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( - std::vector{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( - std::vector{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( - 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; -} diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 0c092ab..143f5b2 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -79,85 +79,4 @@ TrajectoryOptions CreateTrajectoryOptions( CheckTrajectoryOptions(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 diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 97be4c9..93817b7 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -22,7 +22,6 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" -#include "cartographer_ros_msgs/TrajectoryOptions.h" namespace cartographer_ros { @@ -48,25 +47,9 @@ struct TrajectoryOptions { double landmarks_sampling_ratio; }; -::cartographer::mapping::proto::InitialTrajectoryPose -CreateInitialTrajectoryPose( - ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); - TrajectoryOptions CreateTrajectoryOptions( ::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 #endif // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H diff --git a/cartographer_ros_msgs/CMakeLists.txt b/cartographer_ros_msgs/CMakeLists.txt index 15c86de..42d3c97 100644 --- a/cartographer_ros_msgs/CMakeLists.txt +++ b/cartographer_ros_msgs/CMakeLists.txt @@ -33,13 +33,11 @@ add_message_files( MetricFamily.msg MetricLabel.msg Metric.msg - SensorTopics.msg StatusCode.msg StatusResponse.msg SubmapEntry.msg SubmapList.msg SubmapTexture.msg - TrajectoryOptions.msg TrajectoryStates.msg ) diff --git a/cartographer_ros_msgs/msg/SensorTopics.msg b/cartographer_ros_msgs/msg/SensorTopics.msg deleted file mode 100644 index 1db9686..0000000 --- a/cartographer_ros_msgs/msg/SensorTopics.msg +++ /dev/null @@ -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 diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg deleted file mode 100644 index b41810c..0000000 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ /dev/null @@ -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 diff --git a/cartographer_ros_msgs/srv/StartTrajectory.srv b/cartographer_ros_msgs/srv/StartTrajectory.srv index 98e245a..5696635 100644 --- a/cartographer_ros_msgs/srv/StartTrajectory.srv +++ b/cartographer_ros_msgs/srv/StartTrajectory.srv @@ -12,8 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -cartographer_ros_msgs/TrajectoryOptions options -cartographer_ros_msgs/SensorTopics topics +string configuration_directory +string configuration_basename +bool use_initial_pose +geometry_msgs/Pose initial_pose +int32 relative_to_trajectory_id --- cartographer_ros_msgs/StatusResponse status int32 trajectory_id