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.
master
Jihoon Lee 2017-11-24 16:45:11 +01:00 committed by Wally B. Feed
parent 42d82cbdfd
commit e1f65f3104
3 changed files with 52 additions and 1 deletions

View File

@ -37,6 +37,8 @@ DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the " "Basename, i.e. not containing any directory prefix, of the "
"configuration file."); "configuration file.");
DEFINE_string(initial_pose, "", "Starting pose of a new trajectory");
namespace cartographer_ros { namespace cartographer_ros {
namespace { namespace {
@ -49,7 +51,20 @@ TrajectoryOptions LoadOptions() {
auto lua_parameter_dictionary = auto lua_parameter_dictionary =
cartographer::common::LuaParameterDictionary::NonReferenceCounted( cartographer::common::LuaParameterDictionary::NonReferenceCounted(
code, std::move(file_resolver)); 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()); return CreateTrajectoryOptions(lua_parameter_dictionary.get());
}
} }
bool Run() { bool Run() {

View File

@ -17,6 +17,9 @@
#include "cartographer_ros/trajectory_options.h" #include "cartographer_ros/trajectory_options.h"
#include "cartographer/mapping/trajectory_builder.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" #include "glog/logging.h"
namespace cartographer_ros { namespace cartographer_ros {
@ -69,6 +72,31 @@ TrajectoryOptions CreateTrajectoryOptions(
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, bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
TrajectoryOptions* options) { TrajectoryOptions* options) {
options->tracking_frame = msg.tracking_frame; options->tracking_frame = msg.tracking_frame;

View File

@ -43,9 +43,17 @@ struct TrajectoryOptions {
double imu_sampling_ratio; double imu_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. // Try to convert 'msg' into 'options'. Returns false on failure.
bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
TrajectoryOptions* options); TrajectoryOptions* options);