From 5251634d3dce407a41a8a6eb55827b010a2e17d4 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Mon, 26 Feb 2018 12:30:58 +0100 Subject: [PATCH] Add option to publish a pure 2D pose. (#683) If the new `publish_frame_projected_to_2d` option is set to true, the published pose will be restricted to a pure 2D pose (no roll, pitch, or z-offset). This prevents potentially unwanted out-of-plane poses in 2D mode that can occur due to the pose extrapolation step (e.g. if the pose shall be published as a 'base-footprint'-like frame). --- cartographer_ros/cartographer_ros/node.cc | 8 +++++++- cartographer_ros/cartographer_ros/trajectory_options.cc | 4 ++++ cartographer_ros/cartographer_ros/trajectory_options.h | 1 + cartographer_ros/configuration_files/backpack_2d.lua | 1 + cartographer_ros/configuration_files/backpack_3d.lua | 1 + cartographer_ros/configuration_files/pr2.lua | 1 + cartographer_ros/configuration_files/revo_lds.lua | 1 + cartographer_ros/configuration_files/taurob_tracker.lua | 1 + cartographer_ros_msgs/msg/TrajectoryOptions.msg | 1 + 9 files changed, 18 insertions(+), 1 deletion(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index f134f54..10636e7 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -211,7 +211,13 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { const ::cartographer::common::Time now = std::max( FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime()); stamped_transform.header.stamp = ToRos(now); - const Rigid3d tracking_to_local = extrapolator.ExtrapolatePose(now); + + Rigid3d tracking_to_local = extrapolator.ExtrapolatePose(now); + if (trajectory_state.trajectory_options.publish_frame_projected_to_2d) { + tracking_to_local = carto::transform::Embed3D( + carto::transform::Project2D(tracking_to_local)); + } + const Rigid3d tracking_to_map = trajectory_state.local_to_map * tracking_to_local; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 023b6b3..daec52b 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -54,6 +54,8 @@ TrajectoryOptions CreateTrajectoryOptions( lua_parameter_dictionary->GetBool("provide_odom_frame"); options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry"); options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat"); + options.publish_frame_projected_to_2d = + lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d"); options.num_laser_scans = lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans"); options.num_multi_echo_laser_scans = @@ -108,6 +110,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, options->provide_odom_frame = msg.provide_odom_frame; options->use_odometry = msg.use_odometry; options->use_nav_sat = msg.use_nav_sat; + 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 = @@ -136,6 +139,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage( msg.provide_odom_frame = options.provide_odom_frame; msg.use_odometry = options.use_odometry; msg.use_nav_sat = options.use_nav_sat; + 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; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 3702357..fa69f08 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -35,6 +35,7 @@ struct TrajectoryOptions { bool provide_odom_frame; bool use_odometry; bool use_nav_sat; + bool publish_frame_projected_to_2d; int num_laser_scans; int num_multi_echo_laser_scans; int num_subdivisions_per_laser_scan; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 0c6e59e..841b11a 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -23,6 +23,7 @@ options = { published_frame = "base_link", odom_frame = "odom", provide_odom_frame = true, + publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, num_laser_scans = 0, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 1435d72..3dc466c 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -23,6 +23,7 @@ options = { published_frame = "base_link", odom_frame = "odom", provide_odom_frame = true, + publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, num_laser_scans = 0, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index ea0a735..a6c4f4a 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -23,6 +23,7 @@ options = { published_frame = "base_footprint", odom_frame = "odom", provide_odom_frame = true, + publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, num_laser_scans = 1, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 7447760..c16426b 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -23,6 +23,7 @@ options = { published_frame = "horizontal_laser_link", odom_frame = "odom", provide_odom_frame = true, + publish_frame_projected_to_2d = false, use_odometry = false, use_nav_sat = false, num_laser_scans = 1, diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index c98da3a..a66a5c6 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -23,6 +23,7 @@ options = { published_frame = "odom", odom_frame = "odom", provide_odom_frame = false, + publish_frame_projected_to_2d = false, use_odometry = true, use_nav_sat = false, num_laser_scans = 1, diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg index fc92cb9..21017a6 100644 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ b/cartographer_ros_msgs/msg/TrajectoryOptions.msg @@ -18,6 +18,7 @@ string odom_frame bool provide_odom_frame bool use_odometry bool use_nav_sat +bool publish_frame_projected_to_2d int32 num_laser_scans int32 num_multi_echo_laser_scans int32 num_subdivisions_per_laser_scan