Add initial_pose in start_trajectory_main.cc Fixes #579 (#610)

Fixes #579 
Related to googlecartographer/cartographer#606

@damienrg @cschuet I followed most of the comments in googlecartographer/cartographer#642 except timestamp. Receiving timestamp sounds weird to me because trajectory should not start in past timestamp or future timestamp.

cartographer_ros/cartographer_ros/start_trajectory_main.cc

@@ -37,6 +37,8 @@ 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 {
 
@@ -49,8 +51,21 @@ TrajectoryOptions LoadOptions() {
   auto lua_parameter_dictionary =
       cartographer::common::LuaParameterDictionary::NonReferenceCounted(
           code, std::move(file_resolver));
+  if (!FLAGS_initial_pose.empty()) {
+    auto initial_trajectory_pose_file_resolver =
+        cartographer::common::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;

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -17,6 +17,9 @@
 #include "cartographer_ros/trajectory_options.h"
 
 #include "cartographer/mapping/trajectory_builder.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "cartographer_ros/time_conversion.h"
 #include "glog/logging.h"
 
 namespace cartographer_ros {
@@ -69,6 +72,31 @@ TrajectoryOptions CreateTrajectoryOptions(
   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;

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -43,9 +43,17 @@ struct TrajectoryOptions {
   double imu_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);