From 558768f4e30c13d9ef4c2c04c43de20057ebb564 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 3 Aug 2016 16:56:20 +0200 Subject: [PATCH] Fixes pose publishing to include/exclude odom frame. (#3) Changes the pose publishing to always publish the odom to map transform. Either expects or provides (configurable) the odom to tracking transform. Also cleans up the RViz plugin code a bit. --- cartographer_ros/launch/3d_mapping.launch | 2 + cartographer_ros/launch/mapping_2d.launch | 2 + cartographer_ros/launch/state.launch | 73 ------------------- cartographer_ros/launch/turtlebot.launch | 2 + .../src/cartographer_node_main.cc | 53 ++++++++++---- cartographer_ros/src/submaps_display.cc | 45 ++++-------- cartographer_ros/src/submaps_display.h | 8 +- 7 files changed, 59 insertions(+), 126 deletions(-) delete mode 100644 cartographer_ros/launch/state.launch diff --git a/cartographer_ros/launch/3d_mapping.launch b/cartographer_ros/launch/3d_mapping.launch index 45517ba..91099d0 100644 --- a/cartographer_ros/launch/3d_mapping.launch +++ b/cartographer_ros/launch/3d_mapping.launch @@ -84,7 +84,9 @@ type="cartographer_node" output="screen" > map_frame: "map" + odom_frame: "odom" tracking_frame: "base_link" + provide_odom: true configuration_files_directories: [ "$(find cartographer_ros)/configuration_files" ] diff --git a/cartographer_ros/launch/mapping_2d.launch b/cartographer_ros/launch/mapping_2d.launch index f8561ae..e0a80a2 100644 --- a/cartographer_ros/launch/mapping_2d.launch +++ b/cartographer_ros/launch/mapping_2d.launch @@ -25,7 +25,9 @@ type="cartographer_node" output="screen" > map_frame: "map" + odom_frame: "odom" tracking_frame: "base_link" + provide_odom: true configuration_files_directories: [ "$(find cartographer_ros)/configuration_files" ] diff --git a/cartographer_ros/launch/state.launch b/cartographer_ros/launch/state.launch deleted file mode 100644 index 4a931e2..0000000 --- a/cartographer_ros/launch/state.launch +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - # This node publishes the transformation between the tracking and map - # frames. - map_frame: "map" - tracking_frame: "base_link" - - # Lua configuration files are always searched in the cartographer - # installation directory. We also ship configuration that work well for - # the platforms that we used to collect our example data. You probably - # want to add your own configuration overwrites in your own node - # directories - you should add the path here as first entry then. - configuration_files_directories: [ - "$(find cartographer_ros)/configuration_files" - ] - - # Configuration file for SLAM. The settings in here are tweaked to the - # collection platform you are using. - mapping_configuration_basename: "backpack_3d.lua" - - imu_topic: "/imu" - - # Laser min and max range. Everything not in this range will not be used for mapping. - laser_min_range_m: 0. - laser_max_range_m: 30. - - # Missing laser echoes will be inserted as free space at this distance. - laser_missing_echo_ray_length_m: 5. - - # Only choose one in the next parameter block - # laser_scan_2d_topic: "/horizontal_2d_laser" - # multi_echo_laser_scan_2d_topic: "/horizontal_multiecho_2d_laser" - laser_scan_3d_topics: ["/horizontal_3d_laser", "/vertical_3d_laser"] - - - - - diff --git a/cartographer_ros/launch/turtlebot.launch b/cartographer_ros/launch/turtlebot.launch index cfd0c54..15b1a12 100644 --- a/cartographer_ros/launch/turtlebot.launch +++ b/cartographer_ros/launch/turtlebot.launch @@ -25,7 +25,9 @@ type="cartographer_node" output="screen" > map_frame: "map" + odom_frame: "odom" tracking_frame: "base_link" + provide_odom: false configuration_files_directories: [ "$(find cartographer_ros)/configuration_files" ] diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index c6fda22..94d43d8 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -186,7 +186,9 @@ class Node { ros::Subscriber laser_2d_subscriber_; std::vector laser_3d_subscribers_; string tracking_frame_; + string odom_frame_; string map_frame_; + bool provide_odom_; double laser_min_range_m_; double laser_max_range_m_; double laser_missing_echo_ray_length_m_; @@ -249,7 +251,7 @@ void Node::AddImu(const int64 timestamp, const string& frame_id, ::cartographer::transform::ToEigen(imu.linear_acceleration()), sensor_to_tracking.rotation() * ::cartographer::transform::ToEigen(imu.angular_velocity())); - } catch (tf2::TransformException& ex) { + } catch (const tf2::TransformException& ex) { LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ << ": " << ex.what(); } @@ -282,7 +284,7 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id, ::cartographer::sensor::ToLaserFan3D(laser_fan), sensor_to_tracking.cast()); trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d); - } catch (tf2::TransformException& ex) { + } catch (const tf2::TransformException& ex) { LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ << ": " << ex.what(); } @@ -322,7 +324,7 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id, time, ::cartographer::sensor::TransformLaserFan3D( ::cartographer::sensor::FromProto(laser_fan_3d), sensor_to_tracking.cast())); - } catch (tf2::TransformException& ex) { + } catch (const tf2::TransformException& ex) { LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ << ": " << ex.what(); } @@ -339,7 +341,9 @@ const T Node::GetParamOrDie(const string& name) { void Node::Initialize() { tracking_frame_ = GetParamOrDie("tracking_frame"); + odom_frame_ = GetParamOrDie("odom_frame"); map_frame_ = GetParamOrDie("map_frame"); + provide_odom_ = GetParamOrDie("provide_odom"); laser_min_range_m_ = GetParamOrDie("laser_min_range_m"); laser_max_range_m_ = GetParamOrDie("laser_max_range_m"); laser_missing_echo_ray_length_m_ = @@ -515,24 +519,41 @@ void Node::PublishSubmapList(int64 timestamp) { } void Node::PublishPose(int64 timestamp) { - ::cartographer::common::MutexLocker lock(&mutex_); - const ::cartographer::mapping::Submaps* submaps = - trajectory_builder_->submaps(); - const ::cartographer::transform::Rigid3d odometry_to_map = - sparse_pose_graph_->GetOdometryToMapTransform(*submaps); - const auto& pose_estimate = trajectory_builder_->pose_estimate(); - - const ::cartographer::transform::Rigid3d pose = - odometry_to_map * pose_estimate.pose; const ::cartographer::common::Time time = ::cartographer::common::FromUniversal(timestamp); - + const Rigid3d tracking_to_map = trajectory_builder_->pose_estimate().pose; geometry_msgs::TransformStamped stamped_transform; stamped_transform.header.stamp = ToRos(time); stamped_transform.header.frame_id = map_frame_; - stamped_transform.child_frame_id = tracking_frame_; - stamped_transform.transform = ToGeometryMsgTransform(pose); - tf_broadcaster_.sendTransform(stamped_transform); + stamped_transform.child_frame_id = odom_frame_; + + if (provide_odom_) { + ::cartographer::common::MutexLocker lock(&mutex_); + const ::cartographer::mapping::Submaps* submaps = + trajectory_builder_->submaps(); + const ::cartographer::transform::Rigid3d odom_to_map = + sparse_pose_graph_->GetOdometryToMapTransform(*submaps); + stamped_transform.transform = ToGeometryMsgTransform(odom_to_map); + tf_broadcaster_.sendTransform(stamped_transform); + + stamped_transform.header.frame_id = odom_frame_; + stamped_transform.child_frame_id = tracking_frame_; + const ::cartographer::transform::Rigid3d tracking_to_odom = + odom_to_map.inverse() * tracking_to_map; + stamped_transform.transform = ToGeometryMsgTransform(tracking_to_odom); + tf_broadcaster_.sendTransform(stamped_transform); + } else { + try { + const Rigid3d tracking_to_odom = + LookupToTrackingTransformOrThrow(time, odom_frame_).inverse(); + const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse(); + stamped_transform.transform = ToGeometryMsgTransform(odom_to_map); + tf_broadcaster_.sendTransform(stamped_transform); + } catch (const tf2::TransformException& ex) { + LOG(WARNING) << "Cannot transform " << tracking_frame_ << " -> " + << odom_frame_ << ": " << ex.what(); + } + } last_pose_publish_timestamp_ = timestamp; } diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index d062a6f..e59375d 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -79,12 +79,12 @@ SubmapsDisplay::SubmapsDisplay() QString::fromStdString( ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()), "cartographer_ros_msgs::SubmapList topic to subscribe to.", this, - SLOT(UpdateTopic())); + SLOT(UpdateSubmapTopicOrService())); submap_query_service_property_ = new ::rviz::StringProperty( "Submap query service", QString("/cartographer/") + kSubmapQueryServiceName, "Submap query service to connect to.", this, - SLOT(UpdateSubmapQueryServiceName())); + SLOT(UpdateSubmapTopicOrService())); map_frame_property_ = new ::rviz::StringProperty( "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.", this); @@ -104,11 +104,7 @@ SubmapsDisplay::SubmapsDisplay() Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); } -SubmapsDisplay::~SubmapsDisplay() { - Unsubscribe(); - client_.shutdown(); - Clear(); -} +SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); } void SubmapsDisplay::onInitialize() { submaps_scene_manager_ = @@ -138,44 +134,31 @@ void SubmapsDisplay::onInitialize() { kSubmapTexturesGroup); scene_manager_->addListener(&scene_manager_listener_); - // TODO(whess): Combine UpdateTopic()/UpdateSubmapQueryServiceName(). - UpdateSubmapQueryServiceName(); + UpdateSubmapTopicOrService(); } -void SubmapsDisplay::UpdateTopic() { - Unsubscribe(); - Clear(); - Subscribe(); -} - -void SubmapsDisplay::UpdateSubmapQueryServiceName() { - Unsubscribe(); - Clear(); - client_.shutdown(); - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( - submap_query_service_property_->getStdString()); +void SubmapsDisplay::UpdateSubmapTopicOrService() { + UnsubscribeAndClear(); Subscribe(); } void SubmapsDisplay::reset() { Display::reset(); - - Clear(); - UpdateTopic(); + UpdateSubmapTopicOrService(); } void SubmapsDisplay::onEnable() { Subscribe(); } -void SubmapsDisplay::onDisable() { - Unsubscribe(); - Clear(); -} +void SubmapsDisplay::onDisable() { UnsubscribeAndClear(); } void SubmapsDisplay::Subscribe() { if (!isEnabled()) { return; } + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + submap_query_service_property_->getStdString()); + if (!topic_property_->getTopic().isEmpty()) { try { submap_list_subscriber_ = @@ -190,15 +173,15 @@ void SubmapsDisplay::Subscribe() { } } -void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); } - void SubmapsDisplay::IncomingSubmapList( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { submap_list_ = *msg; Q_EMIT SubmapListUpdated(); } -void SubmapsDisplay::Clear() { +void SubmapsDisplay::UnsubscribeAndClear() { + submap_list_subscriber_.shutdown(); + client_.shutdown(); ::cartographer::common::MutexLocker locker(&mutex_); submaps_scene_manager_->clearScene(); if (!rttTexture_.isNull()) { diff --git a/cartographer_ros/src/submaps_display.h b/cartographer_ros/src/submaps_display.h index 41a9f18..48a1585 100644 --- a/cartographer_ros/src/submaps_display.h +++ b/cartographer_ros/src/submaps_display.h @@ -72,8 +72,7 @@ class SubmapsDisplay : public ::rviz::Display { private Q_SLOTS: void RequestNewSubmaps(); - void UpdateTopic(); - void UpdateSubmapQueryServiceName(); + void UpdateSubmapTopicOrService(); private: class SceneManagerListener : public Ogre::SceneManager::Listener { @@ -91,13 +90,10 @@ class SubmapsDisplay : public ::rviz::Display { void onEnable() override; void onDisable() override; void Subscribe(); - void Unsubscribe(); + void UnsubscribeAndClear(); void UpdateMapTexture(); void IncomingSubmapList( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg); - // Clears the current map. - void Clear(); - void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg); int rtt_count_; SceneManagerListener scene_manager_listener_;