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

View File

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

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

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>

View File

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

View File

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

View File

@ -240,6 +240,32 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
return proto; 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( ::cartographer::sensor::proto::LaserFan3D ToCartographer(
const pcl::PointCloud<pcl::PointXYZ>& pcl_points) { const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
::cartographer::sensor::proto::LaserFan3D proto; ::cartographer::sensor::proto::LaserFan3D proto;

View File

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

View File

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

View File

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