12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add code to allow offline processing of 2D Cartographer bags. (#1) 

* Add code to allow offline processing of 2D Cartographer bags.

Fixes waiting for transforms to be published, adds support for multi-echo
laser scans, sets default values for the submap topic and query for the
RViz plugin.
master
Wolfgang Hess 2016-08-03 14:54:45 +02:00 committed by GitHub
parent aabd51e029
commit 961afb6817
9 changed files with 184 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_ros/CMakeLists.txt
View File

@ -97,10 +97,11 @@ target_link_libraries(cartographer_node
add_library(cartographer_rviz_submaps_visualization
src/drawable_submap.cc
src/drawable_submap.h
src/node_constants.h
src/submaps_display.cc
src/submaps_display.h
src/trajectory.h
src/trajectory.cc
src/trajectory.h
)
target_link_libraries(cartographer_rviz_submaps_visualization
${Boost_LIBRARIES}

23
cartographer_ros/configuration_files/backpack_2d.lua Normal file
View File

@ -0,0 +1,23 @@
-- 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.
include "trajectory_builder.lua"
include "sparse_pose_graph.lua"
options = {
sparse_pose_graph = SPARSE_POSE_GRAPH,
trajectory_builder = TRAJECTORY_BUILDER,
}
return options

41
cartographer_ros/launch/mapping_2d.launch Normal file
View File

@ -0,0 +1,41 @@
<!--
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_2d.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">
map_frame: "map"
tracking_frame: "base_link"
configuration_files_directories: [
"$(find cartographer_ros)/configuration_files"
]
mapping_configuration_basename: "backpack_2d.lua"
imu_topic: "/imu"
multi_echo_laser_scan_2d_topic: "/horizontal_2d_laser"
laser_min_range_m: 0.
laser_max_range_m: 30.
laser_missing_echo_ray_length_m: 5.
</rosparam>
</node>
</launch>

130
cartographer_ros/src/cartographer_node_main.cc
View File

@ -42,14 +42,14 @@
#include "cartographer/sensor/proto/sensor.pb.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
#include "glog/log_severity.h"
#include "glog/logging.h"
#include "cartographer_ros_msgs/SubmapEntry.h"
#include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h"
#include "cartographer_ros_msgs/TrajectorySubmapList.h"
#include "geometry_msgs/Transform.h"
#include "geometry_msgs/TransformStamped.h"
#include "glog/log_severity.h"
#include "glog/logging.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
@ -78,6 +78,7 @@ constexpr int64 kTrajectoryId = 0;
constexpr int kSubscriberQueueSize = 150;
constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll; // 300 milliseconds
constexpr int kPosePublishPeriodInUts = 5 * 10000ll; // 5 milliseconds
constexpr double kMaxTransformDelaySeconds = 1.;
Rigid3d ToRidig3d(const geometry_msgs::TransformStamped& transform) {
return Rigid3d(Eigen::Vector3d(transform.transform.translation.x,
@ -165,12 +166,10 @@ class Node {
template <typename T>
const T GetParamOrDie(const string& name);
// Returns true if a transform for 'frame_id' to 'tracking_frame_' exists at
// 'time'.
bool CanTransform(ros::Time time, const string& frame_id);
Rigid3d LookupToTrackingTransformOrDie(ros::Time time,
const string& frame_id);
// Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at
// 'time' or throws tf2::TransformException if it does not exist.
Rigid3d LookupToTrackingTransformOrThrow(::cartographer::common::Time time,
const string& frame_id);
bool HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
@ -217,21 +216,11 @@ Node::Node()
last_submap_list_publish_timestamp_(0),
last_pose_publish_timestamp_(0) {}
bool Node::CanTransform(ros::Time time, const string& frame_id) {
return tf_buffer_.canTransform(tracking_frame_, frame_id, time);
}
Rigid3d Node::LookupToTrackingTransformOrDie(ros::Time time,
const string& frame_id) {
geometry_msgs::TransformStamped stamped_transform;
try {
stamped_transform = tf_buffer_.lookupTransform(tracking_frame_, frame_id,
time, ros::Duration(1.));
} catch (tf2::TransformException& ex) {
LOG(FATAL) << "Timed out while waiting for transform: " << frame_id
<< " -> " << tracking_frame_ << ": " << ex.what();
}
return ToRidig3d(stamped_transform);
Rigid3d Node::LookupToTrackingTransformOrThrow(
const ::cartographer::common::Time time, const string& frame_id) {
return ToRidig3d(
tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time),
ros::Duration(kMaxTransformDelaySeconds)));
}
void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) {
@ -247,22 +236,23 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
const proto::Imu& imu) {
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
if (!CanTransform(ToRos(time), frame_id)) {
LOG(WARNING) << "Cannot transform to " << frame_id;
return;
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
CHECK(sensor_to_tracking.translation().norm() < 1e-5)
<< "The IMU is not colocated with the tracking frame. This makes it "
"hard "
"and inprecise to transform its linear accelaration into the "
"tracking_frame and will decrease the quality of the SLAM.";
trajectory_builder_->AddImuData(
time, sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.linear_acceleration()),
sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.angular_velocity()));
} catch (tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what();
}
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
CHECK(sensor_to_tracking.translation().norm() < 1e-5)
<< "The IMU is not colocated with the tracking frame. This makes it hard "
"and inprecise to transform its linear accelaration into the "
"tracking_frame and will decrease the quality of the SLAM.";
trajectory_builder_->AddImuData(
time, sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.linear_acceleration()),
sensor_to_tracking.rotation() *
::cartographer::transform::ToEigen(imu.angular_velocity()));
}
void Node::LaserScanMessageCallback(
@ -279,29 +269,33 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
const proto::LaserScan& laser_scan) {
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
if (!CanTransform(ToRos(time), frame_id)) {
LOG(WARNING) << "Cannot transform to " << frame_id;
return;
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
// TODO(hrapp): Make things configurable? Through Lua? Through ROS params?
const ::cartographer::sensor::LaserFan laser_fan =
::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_,
laser_max_range_m_,
laser_missing_echo_ray_length_m_);
const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::ToLaserFan3D(laser_fan),
sensor_to_tracking.cast<float>());
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
} catch (tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what();
}
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
// TODO(hrapp): Make things configurable? Through Lua? Through ROS params?
const ::cartographer::sensor::LaserFan laser_fan =
::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_,
laser_max_range_m_,
laser_missing_echo_ray_length_m_);
const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::ToLaserFan3D(laser_fan),
sensor_to_tracking.cast<float>());
trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
}
void Node::MultiEchoLaserScanCallback(
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
// TODO(hrapp): Do something useful.
LOG(INFO) << "LaserScan message: " << msg->header.stamp;
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
msg->header.frame_id, ToCartographer(*msg));
sensor_collator_.AddSensorData(
kTrajectoryId,
::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
laser_2d_subscriber_.getTopic(), std::move(sensor_data));
}
void Node::PointCloud2MessageCallback(
@ -321,17 +315,17 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
const proto::LaserFan3D& laser_fan_3d) {
const ::cartographer::common::Time time =
::cartographer::common::FromUniversal(timestamp);
if (!CanTransform(ToRos(time), frame_id)) {
LOG(WARNING) << "Cannot transform to " << frame_id;
return;
try {
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrThrow(time, frame_id);
trajectory_builder_->AddLaserFan3D(
time, ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::FromProto(laser_fan_3d),
sensor_to_tracking.cast<float>()));
} catch (tf2::TransformException& ex) {
LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
<< ": " << ex.what();
}
const Rigid3d sensor_to_tracking =
LookupToTrackingTransformOrDie(ToRos(time), frame_id);
trajectory_builder_->AddLaserFan3D(
time, ::cartographer::sensor::TransformLaserFan3D(
::cartographer::sensor::FromProto(laser_fan_3d),
sensor_to_tracking.cast<float>()));
}
template <typename T>

6
cartographer_ros/src/drawable_submap.cc
View File

@ -19,8 +19,8 @@
#include <OgreGpuProgramParams.h>
#include <OgreImage.h>
#include <cartographer/common/port.h>
#include <eigen_conversions/eigen_msg.h>
#include <cartographer_ros_msgs/SubmapQuery.h>
#include <eigen_conversions/eigen_msg.h>
#include <ros/ros.h>
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
@ -117,8 +117,8 @@ void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
srv.request.submap_id = submap_id;
srv.request.trajectory_id = trajectory_id;
if (client->call(srv)) {
response_.reset(new ::cartographer_ros_msgs::SubmapQuery::Response(
srv.response));
response_.reset(
new ::cartographer_ros_msgs::SubmapQuery::Response(srv.response));
Q_EMIT RequestSucceeded();
} else {
OnRequestFailure();

26
cartographer_ros/src/msg_conversion.cc
View File

@ -240,6 +240,32 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
return proto;
}
::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::MultiEchoLaserScan& msg) {
::cartographer::sensor::proto::LaserScan proto;
proto.set_angle_min(msg.angle_min);
proto.set_angle_max(msg.angle_max);
proto.set_angle_increment(msg.angle_increment);
proto.set_time_increment(msg.time_increment);
proto.set_scan_time(msg.scan_time);
proto.set_range_min(msg.range_min);
proto.set_range_max(msg.range_max);
for (const auto& range : msg.ranges) {
auto* proto_echoes = proto.add_range()->mutable_value();
for (const auto& echo : range.echoes) {
proto_echoes->Add(echo);
}
}
for (const auto& intensity : msg.intensities) {
auto* proto_echoes = proto.add_intensity()->mutable_value();
for (const auto& echo : intensity.echoes) {
proto_echoes->Add(echo);
}
}
return proto;
}
::cartographer::sensor::proto::LaserFan3D ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
::cartographer::sensor::proto::LaserFan3D proto;

3
cartographer_ros/src/msg_conversion.h
View File

@ -49,6 +49,9 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::LaserScan& msg);
::cartographer::sensor::proto::LaserScan ToCartographer(
const sensor_msgs::MultiEchoLaserScan& msg);
::cartographer::sensor::proto::LaserFan3D ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points);

23
cartographer_ros/src/submaps_display.cc
View File

@ -34,9 +34,9 @@
#include <OgreTextureManager.h>
#include <OgreViewport.h>
#include <cartographer/common/mutex.h>
#include <geometry_msgs/TransformStamped.h>
#include <cartographer_ros_msgs/SubmapList.h>
#include <cartographer_ros_msgs/SubmapQuery.h>
#include <geometry_msgs/TransformStamped.h>
#include <pluginlib/class_list_macros.h>
#include <ros/package.h>
#include <ros/ros.h>
@ -45,6 +45,8 @@
#include <rviz/properties/ros_topic_property.h>
#include <rviz/properties/string_property.h>
#include "node_constants.h"
namespace cartographer_ros {
namespace rviz {
@ -55,8 +57,7 @@ constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
constexpr char kGlsl120Directory[] = "/glsl120";
constexpr char kScriptsDirectory[] = "/scripts";
constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
constexpr char kScreenBlitSourceMaterialName[] =
"cartographer_ros/ScreenBlit";
constexpr char kScreenBlitSourceMaterialName[] = "cartographer_ros/ScreenBlit";
constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
constexpr char kMapTextureName[] = "MapTexture";
constexpr char kMapOverlayName[] = "MapOverlay";
@ -74,13 +75,15 @@ SubmapsDisplay::SubmapsDisplay()
tf_listener_(tf_buffer_) {
connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps()));
topic_property_ = new ::rviz::RosTopicProperty(
"Topic", "",
QString::fromStdString(ros::message_traits::datatype<
::cartographer_ros_msgs::SubmapList>()),
"Topic", QString("/cartographer/") + kSubmapListTopic,
QString::fromStdString(
ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()),
"cartographer_ros_msgs::SubmapList topic to subscribe to.", this,
SLOT(UpdateTopic()));
submap_query_service_property_ = new ::rviz::StringProperty(
"Submap query service", "", "Submap query service to connect to.", this,
"Submap query service",
QString("/cartographer/") + kSubmapQueryServiceName,
"Submap query service to connect to.", this,
SLOT(UpdateSubmapQueryServiceName()));
map_frame_property_ = new ::rviz::StringProperty(
"Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
@ -88,8 +91,7 @@ SubmapsDisplay::SubmapsDisplay()
tracking_frame_property_ = new ::rviz::StringProperty(
"Tracking frame", kDefaultTrackingFrame,
"Tracking frame, used for fading out submaps.", this);
client_ =
update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
@ -136,7 +138,8 @@ void SubmapsDisplay::onInitialize() {
kSubmapTexturesGroup);
scene_manager_->addListener(&scene_manager_listener_);
UpdateTopic();
// TODO(whess): Combine UpdateTopic()/UpdateSubmapQueryServiceName().
UpdateSubmapQueryServiceName();
}
void SubmapsDisplay::UpdateTopic() {

22
cartographer_ros/urdf/backpack_2d.urdf
View File

@ -25,7 +25,7 @@
<color rgba="0.2 0.4 0.2 1"/>
</material>
<link name="/imu_link">
<link name="imu_link">
<visual>
<origin xyz="0.01 0.01 -0.01"/>
<geometry>
@ -35,7 +35,7 @@
</visual>
</link>
<link name="/horizontal_laser_link">
<link name="horizontal_laser_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<geometry>
@ -45,7 +45,7 @@
</visual>
</link>
<link name="/vertical_laser_link">
<link name="vertical_laser_link">
<visual>
<origin xyz="0.0 0.0 -0.01"/>
<geometry>
@ -55,7 +55,7 @@
</visual>
</link>
<link name="/base_footprint">
<link name="base_link">
<visual>
<origin xyz="0.18 0.0 0.2"/>
<geometry>
@ -65,21 +65,21 @@
</visual>
</link>
<joint name="base_link_joint" type="fixed">
<child link="/imu_link"/>
<parent link="/base_footprint"/>
<joint name="imu_link_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="0 0 0" rpy="0 0 3.1416"/>
</joint>
<joint name="horizontal_laser_link_joint" type="fixed">
<parent link="/imu_link"/>
<child link="/horizontal_laser_link"/>
<parent link="base_link"/>
<child link="horizontal_laser_link"/>
<origin xyz="0.1251 0.0937 0.05262"/>
</joint>
<joint name="vertical_laser_link_joint" type="fixed">
<parent link="/imu_link"/>
<child link="/vertical_laser_link"/>
<parent link="base_link"/>
<child link="vertical_laser_link"/>
<origin rpy="0.0 -1.57 3.14" xyz="0.1251 0.0937 0.17772"/>
</joint>
</robot>