Adds a tool to start a new trajectory from an existing Lua file. (#390)

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -17,6 +17,12 @@ google_binary(cartographer_assets_writer
     assets_writer_main.cc
 )
 
+install(TARGETS cartographer_assets_writer
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 google_binary(cartographer_node
   SRCS
     node_main.cc
@@ -28,12 +34,6 @@ install(TARGETS cartographer_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-install(TARGETS cartographer_assets_writer
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
 google_binary(cartographer_offline_node
   SRCS
     offline_node_main.cc
@@ -44,3 +44,15 @@ install(TARGETS cartographer_offline_node
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_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}
+)
+

cartographer_ros/cartographer_ros/node.cc

@@ -53,25 +53,6 @@ namespace {
 constexpr int kInfiniteSubscriberQueueSize = 0;
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 
-// Try to convert 'msg' into 'options'. Returns false on failure.
-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_laser_scan = msg.use_laser_scan;
-  options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
-  options->num_point_clouds = msg.num_point_clouds;
-  if (!options->trajectory_builder_options.ParseFromString(
-          msg.trajectory_builder_options_proto)) {
-    LOG(ERROR) << "Failed to parse protobuf";
-    return false;
-  }
-  return true;
-}
-
 void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
                         int trajectory_id) {
   if (subscribers.count(trajectory_id) == 0) {

cartographer_ros/cartographer_ros/node.h

@@ -22,6 +22,7 @@
 
 #include "cartographer/common/mutex.h"
 #include "cartographer_ros/map_builder_bridge.h"
+#include "cartographer_ros/node_constants.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/trajectory_options.h"
 #include "cartographer_ros_msgs/FinishTrajectory.h"
@@ -37,21 +38,6 @@
 
 namespace cartographer_ros {
 
-// Default topic names; expected to be remapped as needed.
-constexpr char kLaserScanTopic[] = "scan";
-constexpr char kMultiEchoLaserScanTopic[] = "echoes";
-constexpr char kPointCloud2Topic[] = "points2";
-constexpr char kImuTopic[] = "imu";
-constexpr char kOdometryTopic[] = "odom";
-constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
-constexpr char kOccupancyGridTopic[] = "map";
-constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
-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";
-
 // Wires up ROS topics to SLAM.
 class Node {
  public:

cartographer_ros/cartographer_ros/node_constants.h

@@ -0,0 +1,39 @@
+/*
+ * 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.
+ */
+
+#ifndef CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
+#define CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
+
+namespace cartographer_ros {
+
+// Default topic names; expected to be remapped as needed.
+constexpr char kLaserScanTopic[] = "scan";
+constexpr char kMultiEchoLaserScanTopic[] = "echoes";
+constexpr char kPointCloud2Topic[] = "points2";
+constexpr char kImuTopic[] = "imu";
+constexpr char kOdometryTopic[] = "odom";
+constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
+constexpr char kOccupancyGridTopic[] = "map";
+constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
+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";
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_

cartographer_ros/cartographer_ros/start_trajectory_main.cc

@@ -0,0 +1,100 @@
+/*
+ * 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 "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/make_unique.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/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.");
+
+namespace cartographer_ros {
+namespace {
+
+TrajectoryOptions LoadOptions() {
+  auto file_resolver = cartographer::common::make_unique<
+      cartographer::common::ConfigurationFileResolver>(
+      std::vector<string>{FLAGS_configuration_directory});
+  const string code =
+      file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
+  auto lua_parameter_dictionary =
+      cartographer::common::LuaParameterDictionary::NonReferenceCounted(
+          code, std::move(file_resolver));
+  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 = kLaserScanTopic;
+  srv.request.topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
+  srv.request.topics.point_cloud2_topic = kPointCloud2Topic;
+  srv.request.topics.imu_topic = kImuTopic;
+  srv.request.topics.odometry_topic = kOdometryTopic;
+
+  if (!client.call(srv)) {
+    LOG(ERROR) << "Error starting trajectory.";
+    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;
+}

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -50,4 +50,39 @@ TrajectoryOptions CreateTrajectoryOptions(
 
   return options;
 }
+
+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_laser_scan = msg.use_laser_scan;
+  options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
+  options->num_point_clouds = msg.num_point_clouds;
+  if (!options->trajectory_builder_options.ParseFromString(
+          msg.trajectory_builder_options_proto)) {
+    LOG(ERROR) << "Failed to parse protobuf";
+    return false;
+  }
+  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_laser_scan = options.use_laser_scan;
+  msg.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan;
+  msg.num_point_clouds = options.num_point_clouds;
+  options.trajectory_builder_options.SerializeToString(
+      &msg.trajectory_builder_options_proto);
+  return msg;
+}
+
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -22,6 +22,7 @@
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/port.h"
 #include "cartographer_ros/sensor_bridge.h"
+#include "cartographer_ros_msgs/TrajectoryOptions.h"
 
 namespace cartographer_ros {
 
@@ -41,6 +42,14 @@ struct TrajectoryOptions {
 TrajectoryOptions CreateTrajectoryOptions(
     ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
 
+// 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_TRAJECTORY_OPTIONS_H_