参考链接：
- sudo apt-get update
- sudo apt-get install -y google-mock libboost-all-dev libeigen3-dev libgflags-dev libgoogle-glog-dev liblua5.2-dev libprotobuf-dev libsuitesparse-dev libwebp-dev ninja-build protobuf-compiler python-sphinx ros-melodic-tf2-eigen libatlas-base-dev libsuitesparse-dev liblapack-dev
- sudo apt-get install -y python-wstool python-rosdep ninja-build stow
- mkdir catkin_google_ws
- cd catkin_google_ws
- wstool init src
cd src
git clone https://gitee.com/liu_xiao_eu/cartographer.git
git clone https://gitee.com/liu_xiao_eu/cartographer_ros.git
个人感觉没必要在src文件夹下安装，因为安装的ceres-solver会自动写入系统环境中，保持一致，我们在catkin_google_ws/src文件夹下，获取ceres-solver源码，版本要1.13.0。
wget ceres-solver.org/ceres-solver-1.13.0.tar.gz
编译：
- cd ceres-solver-1.13.0
- mkdir build
- cd build
- cmake ..
- make
测试一下，然后安装
- make test
- sudo make install
在工作空间下打开终端输入：
src/cartographer/scripts/install_proto3.sh
- sudo rosdep init
- rosdep update
- rosdep install
src/cartographer/scripts/install_abseil.sh
catkin_make_isolated --install --use-ninja
值得一提的是，在catkin_google_ws工作空间打开终端，刷新环境变量语句是：
source install_isolated/setup.bash
注：安装过程未尽事宜可以参照上面的链接。
基本使用Cartographer需要对三种文件有一个基本了解，即urdf、.lua、.launch文件，下面对这三种文件展开介绍。
简单讲一下，Cartographer需要机器人体包括传感器之间的tf坐标树，在其文件目录中有一个单独的urdf文件夹，我们可以在该目录下创建自己的urdf模型。笔者直接在仿真环境中加载了xacro，所以就没有去构建urdf模型，后续在launch文件中对这一部分进行了注释。
笔者理解该文件是一种配置文件，可以对算法参数进行设置。例如我们进行3D建图，需要用到其中的backpack_3d.lua文件，我们就可以复制该文件然后添加到同一目录下，改个名字，例如叫做xxx_3d.lua。
下面是笔者的xxx_3d.lua文件内容：
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu",
published_frame = "base_footprint",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 1,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_3D.min_range = 0.2
TRAJECTORY_BUILDER_3D.max_range = 150
TRAJECTORY_BUILDER_2D.min_z = 0.1
TRAJECTORY_BUILDER_2D.max_z = 1.0
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = false
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 4
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 20
POSE_GRAPH.constraint_builder.min_score = 0.5
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.55
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e3
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e3
return options
其中参数设置原理可以参照下面的链接：
https://zhuanlan.zhihu.com/p/563264225
https://zhuanlan.zhihu.com/p/563264225
与.lua文件操作相同，我们在这里创建属于自己的launch文件，命名为xxx_3d.launch。
下面是笔者的xxx_3d.launch文件内容：
<launch>
<param name="/use_sim_time" value="true" />
<!-- <param name="robot_description"
textfile="$(find cartographer_ros)/urdf/wangchao_3d.urdf" /> -->
<!-- <node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" /> -->
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename xxx_3d.lua"
output="screen">
<remap from="points2" to="/velodyne_points" />
<remap from="imu" to="/imu/data" />
<remap from="/odom" to="/odom" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
<node name="rviz" pkg="rviz" type="rviz" required="true"
args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
</launch>注：笔者是在仿真环境中进行实验，所以use_sim_time设置为true，并注释了原算法自带的urdf模型代码，点云话题、IMU话题结合自己的实际情况进行修改。此次介绍中.lua和.launch文件仅适用于3D情况。
roslaunch cartographer_ros xxx_3d.launch
启动launch文件前，不要忘记刷新一下环境变量。建图结果如图所示：
tf坐标树关系如下：
注：虽然Cartographer实现了在线建图和定位，但是/odom话题发布和Cartographer没有关系。想要查看Cartographer发布的位姿话题需要我们自行发布，笔者这里发布了carto_odom，来查看Cartographer的定位信息，这将在后续的纯定位环节展开介绍。
Cartographer创建的地图格式与Gmapping、Hector_slam不同，无法调用map_saver节点保存地图，需要以下命令：
首先需要在Cartographer工作空间下打开终端，刷新一下环境变量：
source install_isolated/setup.bash
然后请求/finish_trajectory服务，完成轨迹，不再接收数据，在终端中输入：
rosservice call /finish_trajectory 0
请求/write_state服务，保存当前状态，其中路径可以根据需要自行更改，在终端中输入：
rosservice call /write_state "{filename: '${HOME}/Downloads/mymap.pbstream'}"
笔者当时参考的链接：
见放一下笔者当时参考的链接：
https://blog.csdn.net/qq_40216084/article/details/106086066https://blog.csdn.net/qq_40216084/article/details/106086066注：上面的链接修改了算法中子地图存储数量和构成一次回环的有效帧数量，避免地图覆盖问题，通过减少有效帧数量可以增加回环检测次数，但也增加了计算消耗，读者可以自行调节。
当然保持算法默认参数也是可以的，就只需要参照链接进行如下这步修改，即可使用纯定位：
在这里说一下笔者遇到的特殊情况，因为是仿真环境，笔者的tf树关系如图所示：
刷新后：
xacro模型是没有问题的，问题应该出现在加载差速运动模型xacro时，左右轮和根节点base_footprint的关系由gazebo进行坐标变换发布。
出于此原因，笔者将xxx.lua文件中published_frame 改成了 "odom"，稳定了算法纯定位的实现，机器人模型不再跳动。
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "imu",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = false,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 1,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}roslaunch cartographer_ros xxx_3d.launch load_state_filename:=/homw/xxx/Downloads/mymap1.pbstream
目录
1 Cartographer安装（Ubuntu8.04 melodic）
1.2 安装cartographer、cartographer_ros和ceres_solver
1.2.1 初始化工作空间（这里与参考链接保持一致，命名为catkin_google_ws）
1.2.2 从原作者的gitee上下载安装cartographer和cartographer_ros
1.2.3 在上述创建好的src文件夹下，获取ceres-solver源码
1.2.4 安装cartographer_ros的依赖项（proto3）
1.2.5 安装cartographer_ros的依赖（安装ros时已经执行过没有问题，所以直接进行下一步骤）
1.2.6 安装absell-cpp library，方法同步骤1.2.4，在终端中输入：
3.3.1 修改publish_tracked_pose为true
写到这其实已经写不动了，但是前面留坑需要填。先放笔者当时参考的链接：
https://blog.csdn.net/zhaohaowu/article/details/120889458https://blog.csdn.net/zhaohaowu/article/details/120889458笔者选择了方法1，但是上述代码存在部分问题，一些变量的声明应该是忘记给出了，所以一会直接放代码。
首先在VScode里面ctrl+f找到node.cc里面的node_options_.publish_tracked_pose，选中publish_tracked_pose按F12，跳转到定义处，在node_options.h文件下，可以看到publish_tracked_pose默认为false，将其修改为true，这样算法可以发布tracked_pose话题。
在node.h中，ctrl+f搜索tracked_pose_publisher_，在其下面添加::ros::Publisher pub，具体如图所示：
在第一个if (node_options_.publish_tracked_pose) 中加入：
pub = node_handle_.advertise<::nav_msgs::Odometry>("carto_odom", 100);
具体如图所示：
在第二个if (node_options_.publish_tracked_pose) 中加入：
current_time = ros::Time::now();
double current_x = pose_msg.pose.position.x;
double current_y = pose_msg.pose.position.y;
double current_yaw = tf::getYaw(pose_msg.pose.orientation);
double dt = (current_time - last_time).toSec();
double dx = current_x - last_x;
double dy = current_y - last_y;
double d_yaw = current_yaw - last_yaw;
double lin_speed = dx/dt;
double ang_speed = d_yaw/dt;
::nav_msgs::Odometry msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "odom";
msg.child_frame_id = "velodyne";
msg.pose.pose.position = pose_msg.pose.position;
msg.pose.pose.orientation = pose_msg.pose.orientation;
msg.twist.twist.linear.x = lin_speed;
msg.twist.twist.linear.y = 0.0;
msg.twist.twist.angular.z = ang_speed;
pub.publish(msg);
last_time = ros::Time::now();
double last_x = pose_msg.pose.position.x;
double last_y = pose_msg.pose.position.y;
double last_yaw = tf::getYaw(pose_msg.pose.orientation);具体如图所示：
最后，在node.cc上方补充变量的定义：
如果运行提示tf错误，可以在头文件中添加：
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "tf/tf.h"附上完整的node.cc文件代码：
/*
* 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 "cartographer_ros/node.h"
#include <chrono>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "absl/memory/memory.h"
#include "absl/strings/str_cat.h"
#include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/metrics/register.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "cartographer_ros/metrics/family_factory.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/sensor_bridge.h"
#include "cartographer_ros/tf_bridge.h"
#include "cartographer_ros/time_conversion.h"
#include "cartographer_ros_msgs/StatusCode.h"
#include "cartographer_ros_msgs/StatusResponse.h"
#include "geometry_msgs/PoseStamped.h"
#include "glog/logging.h"
#include "nav_msgs/Odometry.h"
#include "ros/serialization.h"
#include "sensor_msgs/PointCloud2.h"
#include "tf2_eigen/tf2_eigen.h"
#include "visualization_msgs/MarkerArray.h"
//add
ros::Time current_time;
ros::Time last_time;
double last_yaw = 0;
double last_x = 0;
double last_y = 0;
namespace cartographer_ros {
namespace carto = ::cartographer;
using carto::transform::Rigid3d;
using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState;
namespace {
// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and
// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
void (Node::*handler)(int, const std::string&,
const typename MessageType::ConstPtr&),
const int trajectory_id, const std::string& topic,
::ros::NodeHandle* const node_handle, Node* const node) {
return node_handle->subscribe<MessageType>(
topic, kInfiniteSubscriberQueueSize,
boost::function<void(const typename MessageType::ConstPtr&)>(
[node, handler, trajectory_id,
topic](const typename MessageType::ConstPtr& msg) {
(node->*handler)(trajectory_id, topic, msg);
}));
}
std::string TrajectoryStateToString(const TrajectoryState trajectory_state) {
switch (trajectory_state) {
case TrajectoryState::ACTIVE:
return "ACTIVE";
case TrajectoryState::FINISHED:
return "FINISHED";
case TrajectoryState::FROZEN:
return "FROZEN";
case TrajectoryState::DELETED:
return "DELETED";
}
return "";
}
} // namespace
Node::Node(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
absl::MutexLock lock(&mutex_);
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
if (node_options_.publish_tracked_pose) {
tracked_pose_publisher_ =
node_handle_.advertise<::geometry_msgs::PoseStamped>(
kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
//add
pub = node_handle_.advertise<::nav_msgs::Odometry>("carto_odom", 100);
}
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
kReadMetricsServiceName, &Node::HandleReadMetrics, this));
scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));
if (node_options_.pose_publish_period_sec > 0) {
publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishLocalTrajectoryData, this);
}
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this));
}
Node::~Node() { FinishAllTrajectories(); }
::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) {
absl::MutexLock lock(&mutex_);
map_builder_bridge_.HandleSubmapQuery(request, response);
return true;
}
bool Node::HandleTrajectoryQuery(
::cartographer_ros_msgs::TrajectoryQuery::Request& request,
::cartographer_ros_msgs::TrajectoryQuery::Response& response) {
absl::MutexLock lock(&mutex_);
response.status = TrajectoryStateToStatus(
request.trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
TrajectoryState::FROZEN} /* valid states */);
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't query trajectory from pose graph: "
<< response.status.message;
return true;
}
map_builder_bridge_.HandleTrajectoryQuery(request, response);
return true;
}
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
absl::MutexLock lock(&mutex_);
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
}
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
CHECK(extrapolators_.count(trajectory_id) == 0);
const double gravity_time_constant =
node_options_.map_builder_options.use_trajectory_builder_3d()
? options.trajectory_builder_options.trajectory_builder_3d_options()
.imu_gravity_time_constant()
: options.trajectory_builder_options.trajectory_builder_2d_options()
.imu_gravity_time_constant();
extrapolators_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
gravity_time_constant));
}
void Node::AddSensorSamplers(const int trajectory_id,
const TrajectoryOptions& options) {
CHECK(sensor_samplers_.count(trajectory_id) == 0);
sensor_samplers_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
options.landmarks_sampling_ratio));
}
void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
const auto& trajectory_data = entry.second;
auto& extrapolator = extrapolators_.at(entry.first);
// We only publish a point cloud if it has changed. It is not needed at high
// frequency, and republishing it would be computationally wasteful.
if (trajectory_data.local_slam_data->time !=
extrapolator.GetLastPoseTime()) {
if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
// TODO(gaschler): Consider using other message without time
// information.
carto::sensor::TimedPointCloud point_cloud;
point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
.returns.size());
for (const cartographer::sensor::RangefinderPoint point :
trajectory_data.local_slam_data->range_data_in_local.returns) {
point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
point, 0.f /* time */));
}
scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
carto::common::ToUniversal(trajectory_data.local_slam_data->time),
node_options_.map_frame,
carto::sensor::TransformTimedPointCloud(
point_cloud, trajectory_data.local_to_map.cast<float>())));
}
extrapolator.AddPose(trajectory_data.local_slam_data->time,
trajectory_data.local_slam_data->local_pose);
}
geometry_msgs::TransformStamped stamped_transform;
// If we do not publish a new point cloud, we still allow time of the
// published poses to advance. If we already know a newer pose, we use its
// time instead. Since tf knows how to interpolate, providing newer
// information is better.
const ::cartographer::common::Time now = std::max(
FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
stamped_transform.header.stamp =
node_options_.use_pose_extrapolator
? ToRos(now)
: ToRos(trajectory_data.local_slam_data->time);
// Suppress publishing if we already published a transform at this time.
// Due to 2020-07 changes to geometry2, tf buffer will issue warnings for
// repeated transforms with the same timestamp.
if (last_published_tf_stamps_.count(entry.first) &&
last_published_tf_stamps_[entry.first] == stamped_transform.header.stamp)
continue;
last_published_tf_stamps_[entry.first] = stamped_transform.header.stamp;
const Rigid3d tracking_to_local_3d =
node_options_.use_pose_extrapolator
? extrapolator.ExtrapolatePose(now)
: trajectory_data.local_slam_data->local_pose;
const Rigid3d tracking_to_local = [&] {
if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
return carto::transform::Embed3D(
carto::transform::Project2D(tracking_to_local_3d));
}
return tracking_to_local_3d;
}();
const Rigid3d tracking_to_map =
trajectory_data.local_to_map * tracking_to_local;
if (trajectory_data.published_to_tracking != nullptr) {
if (node_options_.publish_to_tf) {
if (trajectory_data.trajectory_options.provide_odom_frame) {
std::vector<geometry_msgs::TransformStamped> stamped_transforms;
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.odom_frame;
stamped_transform.transform =
ToGeometryMsgTransform(trajectory_data.local_to_map);
stamped_transforms.push_back(stamped_transform);
stamped_transform.header.frame_id =
trajectory_data.trajectory_options.odom_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_local * (*trajectory_data.published_to_tracking));
stamped_transforms.push_back(stamped_transform);
tf_broadcaster_.sendTransform(stamped_transforms);
} else {
stamped_transform.header.frame_id = node_options_.map_frame;
stamped_transform.child_frame_id =
trajectory_data.trajectory_options.published_frame;
stamped_transform.transform = ToGeometryMsgTransform(
tracking_to_map * (*trajectory_data.published_to_tracking));
tf_broadcaster_.sendTransform(stamped_transform);
}
}
if (node_options_.publish_tracked_pose) {
::geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = node_options_.map_frame;
pose_msg.header.stamp = stamped_transform.header.stamp;
pose_msg.pose = ToGeometryMsgPose(tracking_to_map);
tracked_pose_publisher_.publish(pose_msg);
//add
current_time = ros::Time::now();
double current_x = pose_msg.pose.position.x;
double current_y = pose_msg.pose.position.y;
double current_yaw = tf::getYaw(pose_msg.pose.orientation);
double dt = (current_time - last_time).toSec();
double dx = current_x - last_x;
double dy = current_y - last_y;
double d_yaw = current_yaw - last_yaw;
double lin_speed = dx/dt;
double ang_speed = d_yaw/dt;
::nav_msgs::Odometry msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "odom";
msg.child_frame_id = "velodyne";
msg.pose.pose.position = pose_msg.pose.position;
msg.pose.pose.orientation = pose_msg.pose.orientation;
msg.twist.twist.linear.x = lin_speed;
msg.twist.twist.linear.y = 0.0;
msg.twist.twist.angular.z = ang_speed;
pub.publish(msg);
last_time = ros::Time::now();
double last_x = pose_msg.pose.position.x;
double last_y = pose_msg.pose.position.y;
double last_yaw = tf::getYaw(pose_msg.pose.orientation);
}
}
}
}
void Node::PublishTrajectoryNodeList(
const ::ros::WallTimerEvent& unused_timer_event) {
if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
absl::MutexLock lock(&mutex_);
trajectory_node_list_publisher_.publish(
map_builder_bridge_.GetTrajectoryNodeList());
}
}
void Node::PublishLandmarkPosesList(
const ::ros::WallTimerEvent& unused_timer_event) {
if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
absl::MutexLock lock(&mutex_);
landmark_poses_list_publisher_.publish(
map_builder_bridge_.GetLandmarkPosesList());
}
}
void Node::PublishConstraintList(
const ::ros::WallTimerEvent& unused_timer_event) {
if (constraint_list_publisher_.getNumSubscribers() > 0) {
absl::MutexLock lock(&mutex_);
constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
}
}
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
using SensorType = SensorId::SensorType;
std::set<SensorId> expected_topics;
// Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
expected_topics.insert(SensorId{SensorType::RANGE, topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
}
// Odometry is optional.
if (options.use_odometry) {
expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
}
// NavSatFix is optional.
if (options.use_nav_sat) {
expected_topics.insert(
SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
}
// Landmark is optional.
if (options.use_landmarks) {
expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
}
return expected_topics;
}
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});
}
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this),
topic});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic,
&node_handle_, this),
topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, kImuTopic,
&node_handle_, this),
kImuTopic});
}
if (options.use_odometry) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, kOdometryTopic,
&node_handle_, this),
kOdometryTopic});
}
if (options.use_nav_sat) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
&node_handle_, this),
kNavSatFixTopic});
}
if (options.use_landmarks) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
&node_handle_, this),
kLandmarkTopic});
}
}
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
return options.trajectory_builder_options
.has_trajectory_builder_2d_options();
}
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
return options.trajectory_builder_options
.has_trajectory_builder_3d_options();
}
return false;
}
bool Node::ValidateTopicNames(const TrajectoryOptions& options) {
for (const auto& sensor_id : ComputeExpectedSensorIds(options)) {
const std::string& topic = sensor_id.id;
if (subscribed_topics_.count(topic) > 0) {
LOG(ERROR) << "Topic name [" << topic << "] is already used.";
return false;
}
}
return true;
}
cartographer_ros_msgs::StatusResponse Node::TrajectoryStateToStatus(
const int trajectory_id, const std::set<TrajectoryState>& valid_states) {
const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
cartographer_ros_msgs::StatusResponse status_response;
const auto it = trajectory_states.find(trajectory_id);
if (it == trajectory_states.end()) {
status_response.message =
absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
return status_response;
}
status_response.message =
absl::StrCat("Trajectory ", trajectory_id, " is in '",
TrajectoryStateToString(it->second), "' state.");
status_response.code =
valid_states.count(it->second)
? cartographer_ros_msgs::StatusCode::OK
: cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
return status_response;
}
cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
const int trajectory_id) {
cartographer_ros_msgs::StatusResponse status_response;
if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
status_response.message = absl::StrCat("Trajectory ", trajectory_id,
" already pending to finish.");
status_response.code = cartographer_ros_msgs::StatusCode::OK;
LOG(INFO) << status_response.message;
return status_response;
}
// First, check if we can actually finish the trajectory.
status_response = TrajectoryStateToStatus(
trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
if (status_response.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't finish trajectory: " << status_response.message;
return status_response;
}
// Shutdown the subscribers of this trajectory.
// A valid case with no subscribers is e.g. if we just visualize states.
if (subscribers_.count(trajectory_id)) {
for (auto& entry : subscribers_[trajectory_id]) {
entry.subscriber.shutdown();
subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
}
CHECK_EQ(subscribers_.erase(trajectory_id), 1);
}
map_builder_bridge_.FinishTrajectory(trajectory_id);
trajectories_scheduled_for_finish_.emplace(trajectory_id);
status_response.message =
absl::StrCat("Finished trajectory ", trajectory_id, ".");
status_response.code = cartographer_ros_msgs::StatusCode::OK;
return status_response;
}
bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) {
TrajectoryOptions trajectory_options;
std::tie(std::ignore, trajectory_options) = LoadOptions(
request.configuration_directory, request.configuration_basename);
if (request.use_initial_pose) {
const auto pose = ToRigid3d(request.initial_pose);
if (!pose.IsValid()) {
response.status.message =
"Invalid pose argument. Orientation quaternion must be normalized.";
LOG(ERROR) << response.status.message;
response.status.code =
cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
return true;
}
// Check if the requested trajectory for the relative initial pose exists.
response.status = TrajectoryStateToStatus(
request.relative_to_trajectory_id,
{TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
TrajectoryState::FINISHED} /* valid states */);
if (response.status.code != cartographer_ros_msgs::StatusCode::OK) {
LOG(ERROR) << "Can't start a trajectory with initial pose: "
<< response.status.message;
return true;
}
::cartographer::mapping::proto::InitialTrajectoryPose
initial_trajectory_pose;
initial_trajectory_pose.set_to_trajectory_id(
request.relative_to_trajectory_id);
*initial_trajectory_pose.mutable_relative_pose() =
cartographer::transform::ToProto(pose);
initial_trajectory_pose.set_timestamp(cartographer::common::ToUniversal(
::cartographer_ros::FromRos(ros::Time(0))));
*trajectory_options.trajectory_builder_options
.mutable_initial_trajectory_pose() = initial_trajectory_pose;
}
if (!ValidateTrajectoryOptions(trajectory_options)) {
response.status.message = "Invalid trajectory options.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
} else if (!ValidateTopicNames(trajectory_options)) {
response.status.message = "Topics are already used by another trajectory.";
LOG(ERROR) << response.status.message;
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
} else {
response.status.message = "Success.";
response.trajectory_id = AddTrajectory(trajectory_options);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
}
return true;
}
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
CHECK(ValidateTrajectoryOptions(options));
AddTrajectory(options);
}
std::vector<
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
Node::ComputeDefaultSensorIdsForMultipleBags(
const std::vector<TrajectoryOptions>& bags_options) const {
using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
std::vector<std::set<SensorId>> bags_sensor_ids;
for (size_t i = 0; i < bags_options.size(); ++i) {
std::string prefix;
if (bags_options.size() > 1) {
prefix = "bag_" + std::to_string(i + 1) + "_";
}
std::set<SensorId> unique_sensor_ids;
for (const auto& sensor_id : ComputeExpectedSensorIds(bags_options.at(i))) {
unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id});
}
bags_sensor_ids.push_back(unique_sensor_ids);
}
return bags_sensor_ids;
}
int Node::AddOfflineTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options);
AddSensorSamplers(trajectory_id, options);
return trajectory_id;
}
bool Node::HandleGetTrajectoryStates(
::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState;
absl::MutexLock lock(&mutex_);
response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
response.trajectory_states.header.stamp = ros::Time::now();
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
response.trajectory_states.trajectory_id.push_back(entry.first);
switch (entry.second) {
case TrajectoryState::ACTIVE:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::ACTIVE);
break;
case TrajectoryState::FINISHED:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::FINISHED);
break;
case TrajectoryState::FROZEN:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::FROZEN);
break;
case TrajectoryState::DELETED:
response.trajectory_states.trajectory_state.push_back(
::cartographer_ros_msgs::TrajectoryStates::DELETED);
break;
}
}
return true;
}
bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
absl::MutexLock lock(&mutex_);
response.status = FinishTrajectoryUnderLock(request.trajectory_id);
return true;
}
bool Node::HandleWriteState(
::cartographer_ros_msgs::WriteState::Request& request,
::cartographer_ros_msgs::WriteState::Response& response) {
absl::MutexLock lock(&mutex_);
if (map_builder_bridge_.SerializeState(request.filename,
request.include_unfinished_submaps)) {
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message =
absl::StrCat("State written to '", request.filename, "'.");
} else {
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
response.status.message =
absl::StrCat("Failed to write '", request.filename, "'.");
}
return true;
}
bool Node::HandleReadMetrics(
::cartographer_ros_msgs::ReadMetrics::Request& request,
::cartographer_ros_msgs::ReadMetrics::Response& response) {
absl::MutexLock lock(&mutex_);
response.timestamp = ros::Time::now();
if (!metrics_registry_) {
response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE;
response.status.message = "Collection of runtime metrics is not activated.";
return true;
}
metrics_registry_->ReadMetrics(&response);
response.status.code = cartographer_ros_msgs::StatusCode::OK;
response.status.message = "Successfully read metrics.";
return true;
}
void Node::FinishAllTrajectories() {
absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
if (entry.second == TrajectoryState::ACTIVE) {
const int trajectory_id = entry.first;
CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
cartographer_ros_msgs::StatusCode::OK);
}
}
}
bool Node::FinishTrajectory(const int trajectory_id) {
absl::MutexLock lock(&mutex_);
return FinishTrajectoryUnderLock(trajectory_id).code ==
cartographer_ros_msgs::StatusCode::OK;
}
void Node::RunFinalOptimization() {
{
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
const int trajectory_id = entry.first;
if (entry.second == TrajectoryState::ACTIVE) {
LOG(WARNING)
<< "Can't run final optimization if there are one or more active "
"trajectories. Trying to finish trajectory with ID "
<< std::to_string(trajectory_id) << " now.";
CHECK(FinishTrajectory(trajectory_id))
<< "Failed to finish trajectory with ID "
<< std::to_string(trajectory_id) << ".";
}
}
}
// Assuming we are not adding new data anymore, the final optimization
// can be performed without holding the mutex.
map_builder_bridge_.RunFinalOptimization();
}
void Node::HandleOdometryMessage(const int trajectory_id,
const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
return;
}
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
if (odometry_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}
void Node::HandleNavSatFixMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleNavSatFixMessage(sensor_id, msg);
}
void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLandmarkMessage(sensor_id, msg);
}
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return;
}
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
void Node::HandleMultiEchoLaserScanMessage(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}
void Node::HandlePointCloud2Message(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandlePointCloud2Message(sensor_id, msg);
}
void Node::SerializeState(const std::string& filename,
const bool include_unfinished_submaps) {
absl::MutexLock lock(&mutex_);
CHECK(
map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
<< "Could not write state.";
}
void Node::LoadState(const std::string& state_filename,
const bool load_frozen_state) {
absl::MutexLock lock(&mutex_);
map_builder_bridge_.LoadState(state_filename, load_frozen_state);
}
void Node::MaybeWarnAboutTopicMismatch(
const ::ros::WallTimerEvent& unused_timer_event) {
::ros::master::V_TopicInfo ros_topics;
::ros::master::getTopics(ros_topics);
std::set<std::string> published_topics;
std::stringstream published_topics_string;
for (const auto& it : ros_topics) {
std::string resolved_topic = node_handle_.resolveName(it.name, false);
published_topics.insert(resolved_topic);
published_topics_string << resolved_topic << ",";
}
bool print_topics = false;
for (const auto& entry : subscribers_) {
int trajectory_id = entry.first;
for (const auto& subscriber : entry.second) {
std::string resolved_topic = node_handle_.resolveName(subscriber.topic);
if (published_topics.count(resolved_topic) == 0) {
LOG(WARNING) << "Expected topic \"" << subscriber.topic
<< "\" (trajectory " << trajectory_id << ")"
<< " (resolved topic \"" << resolved_topic << "\")"
<< " but no publisher is currently active.";
print_topics = true;
}
}
}
if (print_topics) {
LOG(WARNING) << "Currently available topics are: "
<< published_topics_string.str();
}
}
} // namespace cartographer_ros
到此就完成了carto_odom的发布，该话题包含了Cartographer的定位信息。
我需要检查DateTime是否采用有效的ISO8601格式。喜欢:#iso8601?我检查了ruby是否有特定方法，但没有找到。目前我正在使用date.iso8601==date来检查这个。有什么好的方法吗？编辑解释我的环境，并改变问题的范围。因此，我的项目将使用jsapiFullCalendar，这就是我需要iso8601字符串格式的原因。我想知道更好或正确的方法是什么，以正确的格式将日期保存在数据库中，或者让ActiveRecord完成它们的工作并在我需要时间信息时对其进行操作。 最佳答案 我不太明白你的问题。我假设您想检查
无论您是想搭建桌面端、WEB端或者移动端APP应用，HOOPSPlatform组件都可以为您提供弹性的3D集成架构，同时，由工业领域3D技术专家组成的HOOPS技术团队也能为您提供技术支持服务。如果您的客户期望有一种在多个平台（桌面/WEB/APP，而且某些客户端是“瘦”客户端）快速、方便地将数据接入到3D应用系统的解决方案，并且当访问数据时，在各个平台上的性能和用户体验保持一致，HOOPSPlatform将帮助您完成。利用HOOPSPlatform，您可以开发在任何环境下的3D基础应用架构。HOOPSPlatform可以帮您打造3D创新型产品，HOOPSSDK包含的技术有：快速且准确的CAD
本教程将在Unity3D中混合Optitrack与数据手套的数据流，在人体运动的基础上，添加双手手指部分的运动。双手手背的角度仍由Optitrack提供，数据手套提供双手手指的角度。 01 客户端软件分别安装MotiveBody与MotionVenus并校准人体与数据手套。MotiveBodyMotionVenus数据手套使用、校准流程参照：https://gitee.com/foheart_1/foheart-h1-data-summary.git02 数据转发打开MotiveBody软件的Streaming，开始向Unity3D广播数据；MotionVenus中设置->选项选择Unit
Unity自动旋转动画1.开门需要门把手先动,门再动2.关门需要门先动,门把手再动3.中途播放过程中不可以再次进行操作觉得太复杂?查看我的文章开关门简易进阶版效果:如果这个门可以直接打开的话,就不需要放置"门把手"如果门把手还有钥匙需要旋转,那就可以把钥匙放在门把手的"门把手",理论上是可以无限套娃的可调整参数有:角度,反向,轴向,速度运行时点击Test进行测试自己写的代码比较垃圾,命名与结构比较拉,高手轻点喷,新手有类似的需求可以拿去做参考上代码usingSystem.Collections;usingSystem.Collections.Generic;usingUnityEngine;u
之前说过10之后的版本没有3dScan了，所以还是9.8的版本或者之前更早的版本。 3d物体扫描需要先下载扫描的APK进行扫面。首先要在手机上装一个扫描程序，扫描现实中的三维物体，然后上传高通官网，在下载成UnityPackage类型让Unity能够使用这个扫描程序可以从高通官网上进行下载，是一个安卓程序。点到Tools往下滑，找到VuforiaObjectScanner下载后解压数据线连接手机，将apk文件拷入手机安装然后刚才解压文件中的Media文件夹打开，两个PDF图打印第一张A4-ObjectScanningTarget.pdf，主要是用来辅助扫描的。好了，接下来就是扫描三维物体。将瓶
本文主要介绍在使用Selenium进行自动化测试或者任务时，对于使用了iframe的页面，如何定位iframe中的元素文章目录场景描述解决方案具体代码场景描述当我们在使用Selenium进行自动化测试的时候，可能会遇到一些界面或者窗体是使用HTML的iframe标签进行承载的。对于iframe中的标签，如果直接查找是无法找到的，会抛出没有找到元素的异常。比如近在咫尺的例子就是，CSDN的登录窗体就是使用的iframe，大家可以尝试通过F12开发者模式查看到的tag_name,class_name,id或者xpath来定位中的页面元素，会抛出NoSuchElementException异常。解决
我最喜欢的Google文档功能之一是它会在我工作时不断自动保存我的文档版本。这意味着即使我在进行关键更改之前忘记在某个点进行保存，也很有可能会自动创建一个保存点。至少，我可以将文档恢复到错误更改之前的状态，并从该点继续工作。对于在MacOS(或UNIX)上运行的Ruby编码器，是否有具有等效功能的工具？例如，一个工具会每隔几分钟自动将Gitcheckin我的本地存储库以获取我正在处理的文件。也许我有点偏执，但这点小保险可以让我在日常工作中安心。 最佳答案 虚拟机有些人可能讨厌我对此的回应，但我在编码时经常使用VIM，它具有自动保存功
我有一个ActiveRecord对象，我想在不对模型进行永久验证的情况下阻止它被保存。您过去可以使用errors.add执行类似的操作，但它看起来不再有效了。user=User.lastuser.errors.add:name,"namedoesn'trhymewithorange"user.valid?#=>trueuser.save#=>true或user=User.lastuser.errors.add:base,"myuniqueerror"user.valid?#=>trueuser.save#=>true如何在不修改用户对象模型的情况下防止将用户对象保存在Rails3.2中
是否可以为单个ActiveRecord实例添加回调？作为进一步的限制，这是继续使用库，所以我无法控制该类(除了对其进行猴子修补)。这或多或少是我想做的:defdo_something_creazymessage=Message.newmessage.on_save_call:do_even_more_crazy_stuffenddefdo_even_more_crazy_stuff(message)puts"Message#{message}hasbeensaved!Hallelujah!"end 最佳答案 你可以通过在创建对象后立
这就是我做的a="%span.rockets#diamonds.ribbons.forever"a=a.match(/(^\%\w+)([\.|\#]\w+)+/)putsa.inspect这是我得到的#这就是我想要的#帮助？我尝试过但失败了:( 最佳答案 通常，您不能获得任意数量的捕获组，但如果您使用扫描，您可以为您想要捕获的每个标记获得一个匹配:a="%span.rockets#diamonds.ribbons.forever"a=a.scan(/^%\w+|\G[.|#]\w+/)putsa.inspect["%span","