diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 89dde17..178bd90 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -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} diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua new file mode 100644 index 0000000..75b035f --- /dev/null +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -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 diff --git a/cartographer_ros/launch/mapping_2d.launch b/cartographer_ros/launch/mapping_2d.launch new file mode 100644 index 0000000..f8561ae --- /dev/null +++ b/cartographer_ros/launch/mapping_2d.launch @@ -0,0 +1,41 @@ + + + + + + + + + + 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. + + + + diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 7612edb..c6fda22 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -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 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()); + 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()); - 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( + 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())); + } 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())); } template diff --git a/cartographer_ros/src/drawable_submap.cc b/cartographer_ros/src/drawable_submap.cc index c76f63a..c3276eb 100644 --- a/cartographer_ros/src/drawable_submap.cc +++ b/cartographer_ros/src/drawable_submap.cc @@ -19,8 +19,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -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(); diff --git a/cartographer_ros/src/msg_conversion.cc b/cartographer_ros/src/msg_conversion.cc index 31a18e4..e766847 100644 --- a/cartographer_ros/src/msg_conversion.cc +++ b/cartographer_ros/src/msg_conversion.cc @@ -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_points) { ::cartographer::sensor::proto::LaserFan3D proto; diff --git a/cartographer_ros/src/msg_conversion.h b/cartographer_ros/src/msg_conversion.h index f553d04..b069e4d 100644 --- a/cartographer_ros/src/msg_conversion.h +++ b/cartographer_ros/src/msg_conversion.h @@ -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_points); diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index 5b14dca..d062a6f 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -34,9 +34,9 @@ #include #include #include -#include #include #include +#include #include #include #include @@ -45,6 +45,8 @@ #include #include +#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() { diff --git a/cartographer_ros/urdf/backpack_2d.urdf b/cartographer_ros/urdf/backpack_2d.urdf index 3bfdfd9..6c88906 100644 --- a/cartographer_ros/urdf/backpack_2d.urdf +++ b/cartographer_ros/urdf/backpack_2d.urdf @@ -25,7 +25,7 @@ - + @@ -35,7 +35,7 @@ - + @@ -45,7 +45,7 @@ - + @@ -55,7 +55,7 @@ - + @@ -65,21 +65,21 @@ - - - + + + - - + + - - + +