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

Follow Cartographer API. (#283) 

'laser_fan_3d' is renamed 'range_data_3d'.
master
Wolfgang Hess 2017-03-22 13:47:49 +01:00 committed by GitHub
parent 926afcf7ea
commit 2dd30a853b
8 changed files with 35 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -54,30 +54,33 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
carto::io::NullPointsProcessor null_points_processor;
carto::io::XRayPointsProcessor xy_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
{}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
carto::io::XRayPointsProcessor yz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
carto::io::XRayPointsProcessor xz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
voxel_size,
carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
for (const auto& node : trajectory_nodes) {
const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
carto::sensor::Decompress(node.constant_data->laser_fan_3d),
carto::sensor::Decompress(node.constant_data->range_data_3d),
node.pose.cast<float>());
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
points_batch->origin = laser_fan.origin;
points_batch->points = laser_fan.returns;
for (const uint8 reflectivity :
node.constant_data->laser_fan_3d.reflectivities) {
node.constant_data->range_data_3d.reflectivities) {
points_batch->colors.push_back(
carto::io::Color{{reflectivity, reflectivity, reflectivity}});
}

5
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -58,9 +58,8 @@ DEFINE_string(bag_filename, "", "Bag to process.");
DEFINE_string(
trajectory_filename, "",
"Proto containing the trajectory written by /finish_trajectory service.");
DEFINE_bool(
use_bag_transforms, true,
"Whether to read and use the transforms from the bag.");
DEFINE_bool(use_bag_transforms, true,
"Whether to read and use the transforms from the bag.");
namespace cartographer_ros {
namespace {

4
cartographer_ros/cartographer_ros/bag_reader.cc
View File

@ -29,8 +29,8 @@ namespace cartographer_ros {
constexpr char kTfStaticTopic[] = "/tf_static";
void ReadTransformsFromBag(
const string& bag_filename, tf2_ros::Buffer* const tf_buffer) {
void ReadTransformsFromBag(const string& bag_filename,
tf2_ros::Buffer* const tf_buffer) {
rosbag::Bag bag;
bag.open(bag_filename, rosbag::bagmode::Read);
rosbag::View view(bag);

4
cartographer_ros/cartographer_ros/bag_reader.h
View File

@ -22,8 +22,8 @@
namespace cartographer_ros {
void ReadTransformsFromBag(
const string& bag_filename, tf2_ros::Buffer* const tf_buffer);
void ReadTransformsFromBag(const string& bag_filename,
tf2_ros::Buffer* const tf_buffer);
} // namespace cartographer_ros

21
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -148,16 +148,17 @@ void Run() {
kFinishTrajectoryServiceName,
boost::function<bool(
::cartographer_ros_msgs::FinishTrajectory::Request&,
::cartographer_ros_msgs::FinishTrajectory::Response&)>([&](
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response&) {
const int previous_trajectory_id = trajectory_id;
trajectory_id = node.map_builder_bridge()->AddTrajectory(
expected_sensor_ids, options.tracking_frame);
node.map_builder_bridge()->FinishTrajectory(previous_trajectory_id);
node.map_builder_bridge()->WriteAssets(request.stem);
return true;
}));
::cartographer_ros_msgs::FinishTrajectory::Response&)>(
[&](::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response&) {
const int previous_trajectory_id = trajectory_id;
trajectory_id = node.map_builder_bridge()->AddTrajectory(
expected_sensor_ids, options.tracking_frame);
node.map_builder_bridge()->FinishTrajectory(
previous_trajectory_id);
node.map_builder_bridge()->WriteAssets(request.stem);
return true;
}));
::ros::spin();

2
cartographer_ros/cartographer_ros/occupancy_grid.cc
View File

@ -39,7 +39,7 @@ void BuildOccupancyGrid(
carto::mapping_2d::LaserFanInserter laser_fan_inserter(
submaps_options.laser_fan_inserter_options());
for (const auto& node : trajectory_nodes) {
CHECK(node.constant_data->laser_fan_3d.returns.empty()); // No 3D yet.
CHECK(node.constant_data->range_data_3d.returns.empty()); // No 3D yet.
laser_fan_inserter.Insert(
carto::sensor::TransformLaserFan(node.constant_data->laser_fan_2d,
node.pose.cast<float>()),

7
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -47,16 +47,15 @@ DEFINE_string(bag_filenames, "", "Comma-separated list of bags to process.");
DEFINE_string(
urdf_filename, "",
"URDF file that contains static links for your sensor configuration.");
DEFINE_bool(
use_bag_transforms, true,
"Whether to read, use and republish the transforms from the bag.");
DEFINE_bool(use_bag_transforms, true,
"Whether to read, use and republish the transforms from the bag.");
namespace cartographer_ros {
namespace {
constexpr int kLatestOnlyPublisherQueueSize = 1;
constexpr char kClockTopic[] = "clock";
constexpr char kTfTopic [] = "tf";
constexpr char kTfTopic[] = "tf";
volatile std::sig_atomic_t sigint_triggered = 0;

5
cartographer_ros/cartographer_ros/urdf_reader.h
View File

@ -17,14 +17,15 @@
#ifndef CARTOGRAPHER_ROS_URDF_READER_H_
#define CARTOGRAPHER_ROS_URDF_READER_H_
#include <vector>
#include "cartographer/common/port.h"
#include "tf2_ros/buffer.h"
#include <vector>
namespace cartographer_ros {
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
const string& urdf_filename, tf2_ros::Buffer* tf_buffer);
const string& urdf_filename, tf2_ros::Buffer* tf_buffer);
} // namespace cartographer_ros