diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc index 7992d03..ca3d916 100644 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ b/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,7 +51,20 @@ TrajectoryOptions LoadOptions() { auto lua_parameter_dictionary = cartographer::common::LuaParameterDictionary::NonReferenceCounted( code, std::move(file_resolver)); - return CreateTrajectoryOptions(lua_parameter_dictionary.get()); + if (!FLAGS_initial_pose.empty()) { + auto initial_trajectory_pose_file_resolver = + cartographer::common::make_unique< + cartographer::common::ConfigurationFileResolver>( + 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() { diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index a4eb58b..e06463a 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/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; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 82a2926..c7848a8 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/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);