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.
master
Wolfgang Hess 2016-08-03 16:56:20 +02:00 committed by GitHub
parent 3e179972da
commit 558768f4e3
7 changed files with 59 additions and 126 deletions

View File

@ -84,7 +84,9 @@
type="cartographer_node" output="screen" > type="cartographer_node" output="screen" >
<rosparam subst_value="true"> <rosparam subst_value="true">
map_frame: "map" map_frame: "map"
odom_frame: "odom"
tracking_frame: "base_link" tracking_frame: "base_link"
provide_odom: true
configuration_files_directories: [ configuration_files_directories: [
"$(find cartographer_ros)/configuration_files" "$(find cartographer_ros)/configuration_files"
] ]

View File

@ -25,7 +25,9 @@
type="cartographer_node" output="screen" > type="cartographer_node" output="screen" >
<rosparam subst_value="true"> <rosparam subst_value="true">
map_frame: "map" map_frame: "map"
odom_frame: "odom"
tracking_frame: "base_link" tracking_frame: "base_link"
provide_odom: true
configuration_files_directories: [ configuration_files_directories: [
"$(find cartographer_ros)/configuration_files" "$(find cartographer_ros)/configuration_files"
] ]

View File

@ -1,73 +0,0 @@
<!--
Copyright 2016 The Cartographer Authors
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
-->
<launch>
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer" pkg="cartographer_ros"
type="cartographer_node" output="screen" >
<rosparam subst_value="true">
# 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"]
</rosparam>
</node>
<!-- TODO(hrapp): still useful for occasional testing, but delete eventually.
<node pkg="hector_mapping" type="hector_mapping" name="hector_height_mapping"
output="screen">
<param name="scan_topic" value="horizontal_2d_laser" />
<param name="pub_map_odom_transform" value="true"/>
<param name="map_frame" value="map" />
<param name="base_frame" value="base_link" />
<param name="odom_frame" value="base_link" />
<param name="map_size" value="4096" />
<param name="map_resolution" value="0.1" />
</node>
-->
</launch>

View File

@ -25,7 +25,9 @@
type="cartographer_node" output="screen" > type="cartographer_node" output="screen" >
<rosparam subst_value="true"> <rosparam subst_value="true">
map_frame: "map" map_frame: "map"
odom_frame: "odom"
tracking_frame: "base_link" tracking_frame: "base_link"
provide_odom: false
configuration_files_directories: [ configuration_files_directories: [
"$(find cartographer_ros)/configuration_files" "$(find cartographer_ros)/configuration_files"
] ]

View File

@ -186,7 +186,9 @@ class Node {
ros::Subscriber laser_2d_subscriber_; ros::Subscriber laser_2d_subscriber_;
std::vector<ros::Subscriber> laser_3d_subscribers_; std::vector<ros::Subscriber> laser_3d_subscribers_;
string tracking_frame_; string tracking_frame_;
string odom_frame_;
string map_frame_; string map_frame_;
bool provide_odom_;
double laser_min_range_m_; double laser_min_range_m_;
double laser_max_range_m_; double laser_max_range_m_;
double laser_missing_echo_ray_length_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()), ::cartographer::transform::ToEigen(imu.linear_acceleration()),
sensor_to_tracking.rotation() * sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.angular_velocity())); ::cartographer::transform::ToEigen(imu.angular_velocity()));
} catch (tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what(); << ": " << ex.what();
} }
@ -282,7 +284,7 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
::cartographer::sensor::ToLaserFan3D(laser_fan), ::cartographer::sensor::ToLaserFan3D(laser_fan),
sensor_to_tracking.cast<float>()); sensor_to_tracking.cast<float>());
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d); trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
} catch (tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what(); << ": " << ex.what();
} }
@ -322,7 +324,7 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
time, ::cartographer::sensor::TransformLaserFan3D( time, ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::FromProto(laser_fan_3d), ::cartographer::sensor::FromProto(laser_fan_3d),
sensor_to_tracking.cast<float>())); sensor_to_tracking.cast<float>()));
} catch (tf2::TransformException& ex) { } catch (const tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_ LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what(); << ": " << ex.what();
} }
@ -339,7 +341,9 @@ const T Node::GetParamOrDie(const string& name) {
void Node::Initialize() { void Node::Initialize() {
tracking_frame_ = GetParamOrDie<string>("tracking_frame"); tracking_frame_ = GetParamOrDie<string>("tracking_frame");
odom_frame_ = GetParamOrDie<string>("odom_frame");
map_frame_ = GetParamOrDie<string>("map_frame"); map_frame_ = GetParamOrDie<string>("map_frame");
provide_odom_ = GetParamOrDie<bool>("provide_odom");
laser_min_range_m_ = GetParamOrDie<double>("laser_min_range_m"); laser_min_range_m_ = GetParamOrDie<double>("laser_min_range_m");
laser_max_range_m_ = GetParamOrDie<double>("laser_max_range_m"); laser_max_range_m_ = GetParamOrDie<double>("laser_max_range_m");
laser_missing_echo_ray_length_m_ = laser_missing_echo_ray_length_m_ =
@ -515,24 +519,41 @@ void Node::PublishSubmapList(int64 timestamp) {
} }
void Node::PublishPose(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 = const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp); ::cartographer::common::FromUniversal(timestamp);
const Rigid3d tracking_to_map = trajectory_builder_->pose_estimate().pose;
geometry_msgs::TransformStamped stamped_transform; geometry_msgs::TransformStamped stamped_transform;
stamped_transform.header.stamp = ToRos(time); stamped_transform.header.stamp = ToRos(time);
stamped_transform.header.frame_id = map_frame_; stamped_transform.header.frame_id = map_frame_;
stamped_transform.child_frame_id = tracking_frame_; stamped_transform.child_frame_id = odom_frame_;
stamped_transform.transform = ToGeometryMsgTransform(pose);
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); 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; last_pose_publish_timestamp_ = timestamp;
} }

View File

@ -79,12 +79,12 @@ SubmapsDisplay::SubmapsDisplay()
QString::fromStdString( QString::fromStdString(
ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()), ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()),
"cartographer_ros_msgs::SubmapList topic to subscribe to.", this, "cartographer_ros_msgs::SubmapList topic to subscribe to.", this,
SLOT(UpdateTopic())); SLOT(UpdateSubmapTopicOrService()));
submap_query_service_property_ = new ::rviz::StringProperty( submap_query_service_property_ = new ::rviz::StringProperty(
"Submap query service", "Submap query service",
QString("/cartographer/") + kSubmapQueryServiceName, QString("/cartographer/") + kSubmapQueryServiceName,
"Submap query service to connect to.", this, "Submap query service to connect to.", this,
SLOT(UpdateSubmapQueryServiceName())); SLOT(UpdateSubmapTopicOrService()));
map_frame_property_ = new ::rviz::StringProperty( map_frame_property_ = new ::rviz::StringProperty(
"Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.", "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
this); this);
@ -104,11 +104,7 @@ SubmapsDisplay::SubmapsDisplay()
Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
} }
SubmapsDisplay::~SubmapsDisplay() { SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); }
Unsubscribe();
client_.shutdown();
Clear();
}
void SubmapsDisplay::onInitialize() { void SubmapsDisplay::onInitialize() {
submaps_scene_manager_ = submaps_scene_manager_ =
@ -138,44 +134,31 @@ void SubmapsDisplay::onInitialize() {
kSubmapTexturesGroup); kSubmapTexturesGroup);
scene_manager_->addListener(&scene_manager_listener_); scene_manager_->addListener(&scene_manager_listener_);
// TODO(whess): Combine UpdateTopic()/UpdateSubmapQueryServiceName(). UpdateSubmapTopicOrService();
UpdateSubmapQueryServiceName();
} }
void SubmapsDisplay::UpdateTopic() { void SubmapsDisplay::UpdateSubmapTopicOrService() {
Unsubscribe(); UnsubscribeAndClear();
Clear();
Subscribe();
}
void SubmapsDisplay::UpdateSubmapQueryServiceName() {
Unsubscribe();
Clear();
client_.shutdown();
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
submap_query_service_property_->getStdString());
Subscribe(); Subscribe();
} }
void SubmapsDisplay::reset() { void SubmapsDisplay::reset() {
Display::reset(); Display::reset();
UpdateSubmapTopicOrService();
Clear();
UpdateTopic();
} }
void SubmapsDisplay::onEnable() { Subscribe(); } void SubmapsDisplay::onEnable() { Subscribe(); }
void SubmapsDisplay::onDisable() { void SubmapsDisplay::onDisable() { UnsubscribeAndClear(); }
Unsubscribe();
Clear();
}
void SubmapsDisplay::Subscribe() { void SubmapsDisplay::Subscribe() {
if (!isEnabled()) { if (!isEnabled()) {
return; return;
} }
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
submap_query_service_property_->getStdString());
if (!topic_property_->getTopic().isEmpty()) { if (!topic_property_->getTopic().isEmpty()) {
try { try {
submap_list_subscriber_ = submap_list_subscriber_ =
@ -190,15 +173,15 @@ void SubmapsDisplay::Subscribe() {
} }
} }
void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); }
void SubmapsDisplay::IncomingSubmapList( void SubmapsDisplay::IncomingSubmapList(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
submap_list_ = *msg; submap_list_ = *msg;
Q_EMIT SubmapListUpdated(); Q_EMIT SubmapListUpdated();
} }
void SubmapsDisplay::Clear() { void SubmapsDisplay::UnsubscribeAndClear() {
submap_list_subscriber_.shutdown();
client_.shutdown();
::cartographer::common::MutexLocker locker(&mutex_); ::cartographer::common::MutexLocker locker(&mutex_);
submaps_scene_manager_->clearScene(); submaps_scene_manager_->clearScene();
if (!rttTexture_.isNull()) { if (!rttTexture_.isNull()) {

View File

@ -72,8 +72,7 @@ class SubmapsDisplay : public ::rviz::Display {
private Q_SLOTS: private Q_SLOTS:
void RequestNewSubmaps(); void RequestNewSubmaps();
void UpdateTopic(); void UpdateSubmapTopicOrService();
void UpdateSubmapQueryServiceName();
private: private:
class SceneManagerListener : public Ogre::SceneManager::Listener { class SceneManagerListener : public Ogre::SceneManager::Listener {
@ -91,13 +90,10 @@ class SubmapsDisplay : public ::rviz::Display {
void onEnable() override; void onEnable() override;
void onDisable() override; void onDisable() override;
void Subscribe(); void Subscribe();
void Unsubscribe(); void UnsubscribeAndClear();
void UpdateMapTexture(); void UpdateMapTexture();
void IncomingSubmapList( void IncomingSubmapList(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg); const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg);
// Clears the current map.
void Clear();
void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg);
int rtt_count_; int rtt_count_;
SceneManagerListener scene_manager_listener_; SceneManagerListener scene_manager_listener_;