Adds publishing of the scan matched point could. (#53)
parent
f4d69a738b
commit
70f40c6ef2
|
@ -19,8 +19,9 @@ Panels:
|
||||||
Property Tree Widget:
|
Property Tree Widget:
|
||||||
Expanded:
|
Expanded:
|
||||||
- /Submaps1
|
- /Submaps1
|
||||||
|
- /PointCloud21
|
||||||
Splitter Ratio: 0.600671
|
Splitter Ratio: 0.600671
|
||||||
Tree Height: 817
|
Tree Height: 821
|
||||||
- Class: rviz/Selection
|
- Class: rviz/Selection
|
||||||
Name: Selection
|
Name: Selection
|
||||||
- Class: rviz/Tool Properties
|
- Class: rviz/Tool Properties
|
||||||
|
@ -39,7 +40,7 @@ Panels:
|
||||||
Experimental: false
|
Experimental: false
|
||||||
Name: Time
|
Name: Time
|
||||||
SyncMode: 0
|
SyncMode: 0
|
||||||
SyncSource: ""
|
SyncSource: PointCloud2
|
||||||
Visualization Manager:
|
Visualization Manager:
|
||||||
Class: ""
|
Class: ""
|
||||||
Displays:
|
Displays:
|
||||||
|
@ -139,6 +140,36 @@ Visualization Manager:
|
||||||
Tracking frame: base_link
|
Tracking frame: base_link
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Autocompute Intensity Bounds: true
|
||||||
|
Autocompute Value Bounds:
|
||||||
|
Max Value: 10
|
||||||
|
Min Value: -10
|
||||||
|
Value: true
|
||||||
|
Axis: Z
|
||||||
|
Channel Name: intensity
|
||||||
|
Class: rviz/PointCloud2
|
||||||
|
Color: 0; 255; 0
|
||||||
|
Color Transformer: FlatColor
|
||||||
|
Decay Time: 0
|
||||||
|
Enabled: true
|
||||||
|
Invert Rainbow: false
|
||||||
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
|
Name: PointCloud2
|
||||||
|
Position Transformer: XYZ
|
||||||
|
Queue Size: 10
|
||||||
|
Selectable: true
|
||||||
|
Size (Pixels): 3
|
||||||
|
Size (m): 0.05
|
||||||
|
Style: Flat Squares
|
||||||
|
Topic: /scan_matched_points2
|
||||||
|
Unreliable: false
|
||||||
|
Use Fixed Frame: true
|
||||||
|
Use rainbow: true
|
||||||
|
Value: true
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 100; 100; 100
|
Background Color: 100; 100; 100
|
||||||
|
|
|
@ -202,6 +202,36 @@ Visualization Manager:
|
||||||
Tracking frame: base_link
|
Tracking frame: base_link
|
||||||
Unreliable: false
|
Unreliable: false
|
||||||
Value: true
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Autocompute Intensity Bounds: true
|
||||||
|
Autocompute Value Bounds:
|
||||||
|
Max Value: 8.77916
|
||||||
|
Min Value: 4.51287
|
||||||
|
Value: true
|
||||||
|
Axis: Z
|
||||||
|
Channel Name: intensity
|
||||||
|
Class: rviz/PointCloud2
|
||||||
|
Color: 0; 255; 0
|
||||||
|
Color Transformer: AxisColor
|
||||||
|
Decay Time: 0
|
||||||
|
Enabled: false
|
||||||
|
Invert Rainbow: false
|
||||||
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
|
Name: PointCloud2
|
||||||
|
Position Transformer: XYZ
|
||||||
|
Queue Size: 20
|
||||||
|
Selectable: true
|
||||||
|
Size (Pixels): 3
|
||||||
|
Size (m): 0.05
|
||||||
|
Style: Flat Squares
|
||||||
|
Topic: /scan_matched_points2
|
||||||
|
Unreliable: false
|
||||||
|
Use Fixed Frame: true
|
||||||
|
Use rainbow: true
|
||||||
|
Value: false
|
||||||
Enabled: true
|
Enabled: true
|
||||||
Global Options:
|
Global Options:
|
||||||
Background Color: 100; 100; 100
|
Background Color: 100; 100; 100
|
||||||
|
|
|
@ -0,0 +1,317 @@
|
||||||
|
# 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.
|
||||||
|
|
||||||
|
Panels:
|
||||||
|
- Class: rviz/Displays
|
||||||
|
Help Height: 78
|
||||||
|
Name: Displays
|
||||||
|
Property Tree Widget:
|
||||||
|
Expanded:
|
||||||
|
- /Submaps1
|
||||||
|
- /PointCloud21
|
||||||
|
Splitter Ratio: 0.600671
|
||||||
|
Tree Height: 821
|
||||||
|
- Class: rviz/Selection
|
||||||
|
Name: Selection
|
||||||
|
- Class: rviz/Tool Properties
|
||||||
|
Expanded:
|
||||||
|
- /2D Pose Estimate1
|
||||||
|
- /2D Nav Goal1
|
||||||
|
- /Publish Point1
|
||||||
|
Name: Tool Properties
|
||||||
|
Splitter Ratio: 0.588679
|
||||||
|
- Class: rviz/Views
|
||||||
|
Expanded:
|
||||||
|
- /Current View1
|
||||||
|
Name: Views
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
- Class: rviz/Time
|
||||||
|
Experimental: false
|
||||||
|
Name: Time
|
||||||
|
SyncMode: 0
|
||||||
|
SyncSource: PointCloud2
|
||||||
|
Visualization Manager:
|
||||||
|
Class: ""
|
||||||
|
Displays:
|
||||||
|
- Alpha: 0.5
|
||||||
|
Cell Size: 1
|
||||||
|
Class: rviz/Grid
|
||||||
|
Color: 160; 160; 164
|
||||||
|
Enabled: true
|
||||||
|
Line Style:
|
||||||
|
Line Width: 0.03
|
||||||
|
Value: Lines
|
||||||
|
Name: Grid
|
||||||
|
Normal Cell Count: 0
|
||||||
|
Offset:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Plane: XY
|
||||||
|
Plane Cell Count: 100
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Value: true
|
||||||
|
- Class: rviz/TF
|
||||||
|
Enabled: true
|
||||||
|
Frame Timeout: 15
|
||||||
|
Frames:
|
||||||
|
All Enabled: true
|
||||||
|
base_footprint:
|
||||||
|
Value: true
|
||||||
|
base_link:
|
||||||
|
Value: true
|
||||||
|
camera_depth_frame:
|
||||||
|
Value: true
|
||||||
|
camera_depth_optical_frame:
|
||||||
|
Value: true
|
||||||
|
camera_link:
|
||||||
|
Value: true
|
||||||
|
camera_rgb_frame:
|
||||||
|
Value: true
|
||||||
|
camera_rgb_optical_frame:
|
||||||
|
Value: true
|
||||||
|
caster_back_link:
|
||||||
|
Value: true
|
||||||
|
caster_front_link:
|
||||||
|
Value: true
|
||||||
|
cliff_sensor_front_link:
|
||||||
|
Value: true
|
||||||
|
cliff_sensor_left_link:
|
||||||
|
Value: true
|
||||||
|
cliff_sensor_right_link:
|
||||||
|
Value: true
|
||||||
|
gyro_link:
|
||||||
|
Value: true
|
||||||
|
laser:
|
||||||
|
Value: true
|
||||||
|
laser_mount_link:
|
||||||
|
Value: true
|
||||||
|
map:
|
||||||
|
Value: true
|
||||||
|
odom:
|
||||||
|
Value: true
|
||||||
|
plate_bottom_link:
|
||||||
|
Value: true
|
||||||
|
plate_middle_link:
|
||||||
|
Value: true
|
||||||
|
plate_top_link:
|
||||||
|
Value: true
|
||||||
|
pole_bottom_0_link:
|
||||||
|
Value: true
|
||||||
|
pole_bottom_1_link:
|
||||||
|
Value: true
|
||||||
|
pole_bottom_2_link:
|
||||||
|
Value: true
|
||||||
|
pole_bottom_3_link:
|
||||||
|
Value: true
|
||||||
|
pole_bottom_4_link:
|
||||||
|
Value: true
|
||||||
|
pole_bottom_5_link:
|
||||||
|
Value: true
|
||||||
|
pole_kinect_0_link:
|
||||||
|
Value: true
|
||||||
|
pole_kinect_1_link:
|
||||||
|
Value: true
|
||||||
|
pole_middle_0_link:
|
||||||
|
Value: true
|
||||||
|
pole_middle_1_link:
|
||||||
|
Value: true
|
||||||
|
pole_middle_2_link:
|
||||||
|
Value: true
|
||||||
|
pole_middle_3_link:
|
||||||
|
Value: true
|
||||||
|
pole_top_0_link:
|
||||||
|
Value: true
|
||||||
|
pole_top_1_link:
|
||||||
|
Value: true
|
||||||
|
pole_top_2_link:
|
||||||
|
Value: true
|
||||||
|
pole_top_3_link:
|
||||||
|
Value: true
|
||||||
|
wheel_left_link:
|
||||||
|
Value: true
|
||||||
|
wheel_right_link:
|
||||||
|
Value: true
|
||||||
|
Marker Scale: 1
|
||||||
|
Name: TF
|
||||||
|
Show Arrows: true
|
||||||
|
Show Axes: true
|
||||||
|
Show Names: true
|
||||||
|
Tree:
|
||||||
|
map:
|
||||||
|
odom:
|
||||||
|
base_footprint:
|
||||||
|
base_link:
|
||||||
|
camera_rgb_frame:
|
||||||
|
camera_depth_frame:
|
||||||
|
camera_depth_optical_frame:
|
||||||
|
{}
|
||||||
|
camera_link:
|
||||||
|
{}
|
||||||
|
camera_rgb_optical_frame:
|
||||||
|
{}
|
||||||
|
caster_back_link:
|
||||||
|
{}
|
||||||
|
caster_front_link:
|
||||||
|
{}
|
||||||
|
cliff_sensor_front_link:
|
||||||
|
{}
|
||||||
|
cliff_sensor_left_link:
|
||||||
|
{}
|
||||||
|
cliff_sensor_right_link:
|
||||||
|
{}
|
||||||
|
gyro_link:
|
||||||
|
{}
|
||||||
|
laser_mount_link:
|
||||||
|
laser:
|
||||||
|
{}
|
||||||
|
plate_bottom_link:
|
||||||
|
{}
|
||||||
|
plate_middle_link:
|
||||||
|
{}
|
||||||
|
plate_top_link:
|
||||||
|
{}
|
||||||
|
pole_bottom_0_link:
|
||||||
|
{}
|
||||||
|
pole_bottom_1_link:
|
||||||
|
{}
|
||||||
|
pole_bottom_2_link:
|
||||||
|
{}
|
||||||
|
pole_bottom_3_link:
|
||||||
|
{}
|
||||||
|
pole_bottom_4_link:
|
||||||
|
{}
|
||||||
|
pole_bottom_5_link:
|
||||||
|
{}
|
||||||
|
pole_kinect_0_link:
|
||||||
|
{}
|
||||||
|
pole_kinect_1_link:
|
||||||
|
{}
|
||||||
|
pole_middle_0_link:
|
||||||
|
{}
|
||||||
|
pole_middle_1_link:
|
||||||
|
{}
|
||||||
|
pole_middle_2_link:
|
||||||
|
{}
|
||||||
|
pole_middle_3_link:
|
||||||
|
{}
|
||||||
|
pole_top_0_link:
|
||||||
|
{}
|
||||||
|
pole_top_1_link:
|
||||||
|
{}
|
||||||
|
pole_top_2_link:
|
||||||
|
{}
|
||||||
|
pole_top_3_link:
|
||||||
|
{}
|
||||||
|
wheel_left_link:
|
||||||
|
{}
|
||||||
|
wheel_right_link:
|
||||||
|
{}
|
||||||
|
Update Interval: 0
|
||||||
|
Value: true
|
||||||
|
- Class: Submaps
|
||||||
|
Enabled: true
|
||||||
|
Map frame: map
|
||||||
|
Name: Submaps
|
||||||
|
Submap query service: /cartographer/submap_query
|
||||||
|
Topic: /cartographer/submap_list
|
||||||
|
Tracking frame: base_link
|
||||||
|
Unreliable: false
|
||||||
|
Value: true
|
||||||
|
- Alpha: 1
|
||||||
|
Autocompute Intensity Bounds: true
|
||||||
|
Autocompute Value Bounds:
|
||||||
|
Max Value: 10
|
||||||
|
Min Value: -10
|
||||||
|
Value: true
|
||||||
|
Axis: Z
|
||||||
|
Channel Name: intensity
|
||||||
|
Class: rviz/PointCloud2
|
||||||
|
Color: 0; 255; 0
|
||||||
|
Color Transformer: FlatColor
|
||||||
|
Decay Time: 0
|
||||||
|
Enabled: true
|
||||||
|
Invert Rainbow: false
|
||||||
|
Max Color: 255; 255; 255
|
||||||
|
Max Intensity: 4096
|
||||||
|
Min Color: 0; 0; 0
|
||||||
|
Min Intensity: 0
|
||||||
|
Name: PointCloud2
|
||||||
|
Position Transformer: XYZ
|
||||||
|
Queue Size: 10
|
||||||
|
Selectable: true
|
||||||
|
Size (Pixels): 3
|
||||||
|
Size (m): 0.05
|
||||||
|
Style: Flat Squares
|
||||||
|
Topic: /scan_matched_points2
|
||||||
|
Unreliable: false
|
||||||
|
Use Fixed Frame: true
|
||||||
|
Use rainbow: true
|
||||||
|
Value: true
|
||||||
|
Enabled: true
|
||||||
|
Global Options:
|
||||||
|
Background Color: 100; 100; 100
|
||||||
|
Fixed Frame: map
|
||||||
|
Frame Rate: 30
|
||||||
|
Name: root
|
||||||
|
Tools:
|
||||||
|
- Class: rviz/Interact
|
||||||
|
Hide Inactive Objects: true
|
||||||
|
- Class: rviz/MoveCamera
|
||||||
|
- Class: rviz/Select
|
||||||
|
- Class: rviz/FocusCamera
|
||||||
|
- Class: rviz/Measure
|
||||||
|
- Class: rviz/SetInitialPose
|
||||||
|
Topic: /initialpose
|
||||||
|
- Class: rviz/SetGoal
|
||||||
|
Topic: /move_base_simple/goal
|
||||||
|
- Class: rviz/PublishPoint
|
||||||
|
Single click: true
|
||||||
|
Topic: /clicked_point
|
||||||
|
Value: true
|
||||||
|
Views:
|
||||||
|
Current:
|
||||||
|
Angle: 0
|
||||||
|
Class: rviz/TopDownOrtho
|
||||||
|
Enable Stereo Rendering:
|
||||||
|
Stereo Eye Separation: 0.06
|
||||||
|
Stereo Focal Distance: 1
|
||||||
|
Swap Stereo Eyes: false
|
||||||
|
Value: false
|
||||||
|
Name: Current View
|
||||||
|
Near Clip Distance: 0.01
|
||||||
|
Scale: 10
|
||||||
|
Target Frame: <Fixed Frame>
|
||||||
|
Value: TopDownOrtho (rviz)
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Saved: ~
|
||||||
|
Window Geometry:
|
||||||
|
Displays:
|
||||||
|
collapsed: false
|
||||||
|
Height: 1123
|
||||||
|
Hide Left Dock: false
|
||||||
|
Hide Right Dock: false
|
||||||
|
QMainWindow State: 000000ff00000000fd0000000400000000000001c5000003c0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000003c0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000041000003c0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000049e000003c000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
|
Selection:
|
||||||
|
collapsed: false
|
||||||
|
Time:
|
||||||
|
collapsed: false
|
||||||
|
Tool Properties:
|
||||||
|
collapsed: false
|
||||||
|
Views:
|
||||||
|
collapsed: false
|
||||||
|
Width: 1918
|
||||||
|
X: 0
|
||||||
|
Y: 24
|
|
@ -0,0 +1,29 @@
|
||||||
|
<!--
|
||||||
|
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="/use_sim_time" value="true" />
|
||||||
|
|
||||||
|
<node name="cartographer" pkg="cartographer_ros"
|
||||||
|
type="cartographer_node" args="
|
||||||
|
-configuration_directory $(find cartographer_ros)/configuration_files
|
||||||
|
-configuration_basename turtlebot.lua"
|
||||||
|
output="screen" />
|
||||||
|
<node name="rviz" pkg="rviz" type="rviz" required="true"
|
||||||
|
args="-d $(find cartographer_ros)/configuration_files/demo_turtlebot.rviz" />
|
||||||
|
<node name="playbag" pkg="rosbag" type="play"
|
||||||
|
args="--clock $(arg bag_filename)" />
|
||||||
|
</launch>
|
|
@ -43,6 +43,7 @@
|
||||||
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
#include "cartographer/mapping_3d/local_trajectory_builder_options.h"
|
||||||
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
#include "cartographer/mapping_3d/sparse_pose_graph.h"
|
||||||
#include "cartographer/sensor/laser.h"
|
#include "cartographer/sensor/laser.h"
|
||||||
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#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"
|
||||||
|
@ -97,6 +98,7 @@ constexpr char kPointCloud2Topic[] = "/points2";
|
||||||
constexpr char kImuTopic[] = "/imu";
|
constexpr char kImuTopic[] = "/imu";
|
||||||
constexpr char kOdometryTopic[] = "/odom";
|
constexpr char kOdometryTopic[] = "/odom";
|
||||||
constexpr char kOccupancyGridTopic[] = "/map";
|
constexpr char kOccupancyGridTopic[] = "/map";
|
||||||
|
constexpr char kScanMatchedPointCloudTopic[] = "/scan_matched_points2";
|
||||||
|
|
||||||
struct NodeOptions {
|
struct NodeOptions {
|
||||||
carto::mapping::proto::MapBuilderOptions map_builder_options;
|
carto::mapping::proto::MapBuilderOptions map_builder_options;
|
||||||
|
@ -206,7 +208,8 @@ class Node {
|
||||||
::cartographer_ros_msgs::SubmapQuery::Response& response);
|
::cartographer_ros_msgs::SubmapQuery::Response& response);
|
||||||
|
|
||||||
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
|
void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
|
||||||
void PublishPose(const ::ros::WallTimerEvent& timer_event);
|
void PublishPoseAndScanMatchedPointCloud(
|
||||||
|
const ::ros::WallTimerEvent& timer_event);
|
||||||
void SpinOccupancyGridThreadForever();
|
void SpinOccupancyGridThreadForever();
|
||||||
|
|
||||||
const NodeOptions options_;
|
const NodeOptions options_;
|
||||||
|
@ -226,6 +229,7 @@ class Node {
|
||||||
::ros::Subscriber odometry_subscriber_;
|
::ros::Subscriber odometry_subscriber_;
|
||||||
::ros::Publisher submap_list_publisher_;
|
::ros::Publisher submap_list_publisher_;
|
||||||
::ros::ServiceServer submap_query_server_;
|
::ros::ServiceServer submap_query_server_;
|
||||||
|
::ros::Publisher scan_matched_point_cloud_publisher_;
|
||||||
|
|
||||||
tf2_ros::Buffer tf_buffer_;
|
tf2_ros::Buffer tf_buffer_;
|
||||||
tf2_ros::TransformListener tf_;
|
tf2_ros::TransformListener tf_;
|
||||||
|
@ -426,12 +430,16 @@ void Node::Initialize() {
|
||||||
std::thread(&Node::SpinOccupancyGridThreadForever, this);
|
std::thread(&Node::SpinOccupancyGridThreadForever, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
scan_matched_point_cloud_publisher_ =
|
||||||
|
node_handle_.advertise<sensor_msgs::PointCloud2>(
|
||||||
|
kScanMatchedPointCloudTopic, 10);
|
||||||
|
|
||||||
wall_timers_.push_back(node_handle_.createWallTimer(
|
wall_timers_.push_back(node_handle_.createWallTimer(
|
||||||
::ros::WallDuration(options_.submap_publish_period_sec),
|
::ros::WallDuration(options_.submap_publish_period_sec),
|
||||||
&Node::PublishSubmapList, this));
|
&Node::PublishSubmapList, this));
|
||||||
wall_timers_.push_back(node_handle_.createWallTimer(
|
wall_timers_.push_back(node_handle_.createWallTimer(
|
||||||
::ros::WallDuration(options_.pose_publish_period_sec), &Node::PublishPose,
|
::ros::WallDuration(options_.pose_publish_period_sec),
|
||||||
this));
|
&Node::PublishPoseAndScanMatchedPointCloud, this));
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Node::HandleSubmapQuery(
|
bool Node::HandleSubmapQuery(
|
||||||
|
@ -508,21 +516,26 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& timer_event) {
|
||||||
submap_list_publisher_.publish(ros_submap_list);
|
submap_list_publisher_.publish(ros_submap_list);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::PublishPose(const ::ros::WallTimerEvent& timer_event) {
|
void Node::PublishPoseAndScanMatchedPointCloud(
|
||||||
|
const ::ros::WallTimerEvent& timer_event) {
|
||||||
carto::common::MutexLocker lock(&mutex_);
|
carto::common::MutexLocker lock(&mutex_);
|
||||||
const Rigid3d tracking_to_local =
|
const carto::mapping::GlobalTrajectoryBuilderInterface::PoseEstimate
|
||||||
|
last_pose_estimate =
|
||||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
|
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
|
||||||
->pose_estimate()
|
->pose_estimate();
|
||||||
.pose;
|
if (carto::common::ToUniversal(last_pose_estimate.time) < 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const Rigid3d tracking_to_local = last_pose_estimate.pose;
|
||||||
const carto::mapping::Submaps* submaps =
|
const carto::mapping::Submaps* submaps =
|
||||||
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
|
map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)->submaps();
|
||||||
const Rigid3d local_to_map =
|
const Rigid3d local_to_map =
|
||||||
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(*submaps);
|
map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(*submaps);
|
||||||
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
|
const Rigid3d tracking_to_map = local_to_map * tracking_to_local;
|
||||||
|
|
||||||
const ::ros::Time now = ::ros::Time::now();
|
|
||||||
geometry_msgs::TransformStamped stamped_transform;
|
geometry_msgs::TransformStamped stamped_transform;
|
||||||
stamped_transform.header.stamp = now;
|
stamped_transform.header.stamp = ToRos(last_pose_estimate.time);
|
||||||
stamped_transform.header.frame_id = options_.map_frame;
|
stamped_transform.header.frame_id = options_.map_frame;
|
||||||
stamped_transform.child_frame_id = options_.odom_frame;
|
stamped_transform.child_frame_id = options_.odom_frame;
|
||||||
|
|
||||||
|
@ -537,7 +550,8 @@ void Node::PublishPose(const ::ros::WallTimerEvent& timer_event) {
|
||||||
} else {
|
} else {
|
||||||
try {
|
try {
|
||||||
const Rigid3d tracking_to_odom =
|
const Rigid3d tracking_to_odom =
|
||||||
LookupToTrackingTransformOrThrow(FromRos(now), options_.odom_frame)
|
LookupToTrackingTransformOrThrow(last_pose_estimate.time,
|
||||||
|
options_.odom_frame)
|
||||||
.inverse();
|
.inverse();
|
||||||
const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse();
|
const Rigid3d odom_to_map = tracking_to_map * tracking_to_odom.inverse();
|
||||||
stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
|
stamped_transform.transform = ToGeometryMsgTransform(odom_to_map);
|
||||||
|
@ -547,6 +561,12 @@ void Node::PublishPose(const ::ros::WallTimerEvent& timer_event) {
|
||||||
<< options_.odom_frame << ": " << ex.what();
|
<< options_.odom_frame << ": " << ex.what();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
|
||||||
|
carto::common::ToUniversal(last_pose_estimate.time), options_.tracking_frame,
|
||||||
|
carto::sensor::TransformPointCloud(
|
||||||
|
last_pose_estimate.point_cloud,
|
||||||
|
tracking_to_local.inverse().cast<float>())));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::SpinOccupancyGridThreadForever() {
|
void Node::SpinOccupancyGridThreadForever() {
|
||||||
|
|
|
@ -37,6 +37,7 @@
|
||||||
#include "time_conversion.h"
|
#include "time_conversion.h"
|
||||||
|
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
|
|
||||||
// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
|
// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
|
||||||
|
@ -62,6 +63,35 @@ void ToMessage(const ::cartographer::transform::proto::Quaterniond& proto,
|
||||||
quaternion->z = proto.z();
|
quaternion->z = proto.z();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp,
|
||||||
|
const string& frame_id,
|
||||||
|
const int num_points) {
|
||||||
|
sensor_msgs::PointCloud2 msg;
|
||||||
|
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
||||||
|
msg.header.frame_id = frame_id;
|
||||||
|
msg.height = 1;
|
||||||
|
msg.width = num_points;
|
||||||
|
msg.fields.resize(3);
|
||||||
|
msg.fields[0].name = "x";
|
||||||
|
msg.fields[0].offset = 0;
|
||||||
|
msg.fields[0].datatype = 7;
|
||||||
|
msg.fields[0].count = 1;
|
||||||
|
msg.fields[1].name = "y";
|
||||||
|
msg.fields[1].offset = 4;
|
||||||
|
msg.fields[1].datatype = 7;
|
||||||
|
msg.fields[1].count = 1;
|
||||||
|
msg.fields[2].name = "z";
|
||||||
|
msg.fields[2].offset = 8;
|
||||||
|
msg.fields[2].datatype = 7;
|
||||||
|
msg.fields[2].count = 1;
|
||||||
|
msg.is_bigendian = false;
|
||||||
|
msg.point_step = 16;
|
||||||
|
msg.row_step = 16 * msg.width;
|
||||||
|
msg.is_dense = true;
|
||||||
|
msg.data.resize(16 * num_points);
|
||||||
|
return msg;
|
||||||
|
}
|
||||||
|
|
||||||
} // namespace
|
} // namespace
|
||||||
|
|
||||||
sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
|
sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
|
||||||
|
@ -149,42 +179,32 @@ sensor_msgs::LaserScan ToLaserScan(
|
||||||
return msg;
|
return msg;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
|
const int64 timestamp, const string& frame_id,
|
||||||
|
const ::cartographer::sensor::PointCloud& point_cloud) {
|
||||||
|
auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
|
||||||
|
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
|
||||||
|
for (const auto& point : point_cloud) {
|
||||||
|
stream.next(point.x());
|
||||||
|
stream.next(point.y());
|
||||||
|
stream.next(point.z());
|
||||||
|
stream.next(kPointCloudComponentFourMagic);
|
||||||
|
}
|
||||||
|
return msg;
|
||||||
|
}
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
const int64 timestamp, const string& frame_id,
|
const int64 timestamp, const string& frame_id,
|
||||||
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) {
|
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d) {
|
||||||
CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0)
|
CHECK(::cartographer::transform::ToEigen(laser_scan_3d.origin()).norm() == 0)
|
||||||
<< "Trying to convert a laser_scan_3d that is not at the origin.";
|
<< "Trying to convert a laser_scan_3d that is not at the origin.";
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 msg;
|
|
||||||
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
|
|
||||||
msg.header.frame_id = frame_id;
|
|
||||||
|
|
||||||
const auto& point_cloud = laser_scan_3d.point_cloud();
|
const auto& point_cloud = laser_scan_3d.point_cloud();
|
||||||
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
|
CHECK_EQ(point_cloud.x_size(), point_cloud.y_size());
|
||||||
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
|
CHECK_EQ(point_cloud.x_size(), point_cloud.z_size());
|
||||||
const auto num_points = point_cloud.x_size();
|
const auto num_points = point_cloud.x_size();
|
||||||
|
|
||||||
msg.height = 1;
|
auto msg = PreparePointCloud2Message(timestamp, frame_id, num_points);
|
||||||
msg.width = num_points;
|
|
||||||
msg.fields.resize(3);
|
|
||||||
msg.fields[0].name = "x";
|
|
||||||
msg.fields[0].offset = 0;
|
|
||||||
msg.fields[0].datatype = 7;
|
|
||||||
msg.fields[0].count = 1;
|
|
||||||
msg.fields[1].name = "y";
|
|
||||||
msg.fields[1].offset = 4;
|
|
||||||
msg.fields[1].datatype = 7;
|
|
||||||
msg.fields[1].count = 1;
|
|
||||||
msg.fields[2].name = "z";
|
|
||||||
msg.fields[2].offset = 8;
|
|
||||||
msg.fields[2].datatype = 7;
|
|
||||||
msg.fields[2].count = 1;
|
|
||||||
msg.is_bigendian = false;
|
|
||||||
msg.point_step = 16;
|
|
||||||
msg.row_step = 16 * msg.width;
|
|
||||||
msg.is_dense = true;
|
|
||||||
msg.data.resize(16 * num_points);
|
|
||||||
|
|
||||||
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
|
::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
|
||||||
for (int i = 0; i < num_points; ++i) {
|
for (int i = 0; i < num_points; ++i) {
|
||||||
stream.next(point_cloud.x(i));
|
stream.next(point_cloud.x(i));
|
||||||
|
|
|
@ -19,6 +19,7 @@
|
||||||
|
|
||||||
#include "cartographer/common/port.h"
|
#include "cartographer/common/port.h"
|
||||||
#include "cartographer/kalman_filter/pose_tracker.h"
|
#include "cartographer/kalman_filter/pose_tracker.h"
|
||||||
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
#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 "geometry_msgs/Pose.h"
|
#include "geometry_msgs/Pose.h"
|
||||||
|
@ -34,7 +35,7 @@
|
||||||
|
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
// Only uses first echo of each beam.
|
// Returns a laser scan consisting of the first echo of each beam.
|
||||||
sensor_msgs::LaserScan ToLaserScan(
|
sensor_msgs::LaserScan ToLaserScan(
|
||||||
int64 timestamp, const string& frame_id,
|
int64 timestamp, const string& frame_id,
|
||||||
const ::cartographer::sensor::proto::LaserScan& laser_scan);
|
const ::cartographer::sensor::proto::LaserScan& laser_scan);
|
||||||
|
@ -46,6 +47,10 @@ sensor_msgs::MultiEchoLaserScan ToMultiEchoLaserScanMessage(
|
||||||
sensor_msgs::Imu ToImuMessage(int64 timestamp, const string& frame_id,
|
sensor_msgs::Imu ToImuMessage(int64 timestamp, const string& frame_id,
|
||||||
const ::cartographer::sensor::proto::Imu& imu);
|
const ::cartographer::sensor::proto::Imu& imu);
|
||||||
|
|
||||||
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
|
int64 timestamp, const string& frame_id,
|
||||||
|
const ::cartographer::sensor::PointCloud& point_cloud);
|
||||||
|
|
||||||
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
sensor_msgs::PointCloud2 ToPointCloud2Message(
|
||||||
int64 timestamp, const string& frame_id,
|
int64 timestamp, const string& frame_id,
|
||||||
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d);
|
const ::cartographer::sensor::proto::LaserFan3D& laser_scan_3d);
|
||||||
|
|
Loading…
Reference in New Issue