Renames SensorDataProducer to SensorBridge. (#90)
parent
0c8d6afc33
commit
a02dfa084a
|
@ -91,10 +91,10 @@ add_executable(cartographer_node
|
|||
src/occupancy_grid.h
|
||||
src/ros_log_sink.cc
|
||||
src/ros_log_sink.h
|
||||
src/sensor_bridge.cc
|
||||
src/sensor_bridge.h
|
||||
src/sensor_data.cc
|
||||
src/sensor_data.h
|
||||
src/sensor_data_producer.cc
|
||||
src/sensor_data_producer.h
|
||||
src/time_conversion.cc
|
||||
src/time_conversion.h
|
||||
)
|
||||
|
|
|
@ -67,8 +67,8 @@
|
|||
#include "node_options.h"
|
||||
#include "occupancy_grid.h"
|
||||
#include "ros_log_sink.h"
|
||||
#include "sensor_bridge.h"
|
||||
#include "sensor_data.h"
|
||||
#include "sensor_data_producer.h"
|
||||
#include "time_conversion.h"
|
||||
|
||||
DEFINE_string(configuration_directory, "",
|
||||
|
@ -156,7 +156,7 @@ class Node {
|
|||
carto::mapping::MapBuilder map_builder_ GUARDED_BY(mutex_);
|
||||
carto::mapping::SensorCollator<SensorData> sensor_collator_
|
||||
GUARDED_BY(mutex_);
|
||||
SensorDataProducer sensor_data_producer_ GUARDED_BY(mutex_);
|
||||
SensorBridge sensor_bridge_ GUARDED_BY(mutex_);
|
||||
|
||||
::ros::NodeHandle node_handle_;
|
||||
::ros::Subscriber imu_subscriber_;
|
||||
|
@ -190,7 +190,7 @@ class Node {
|
|||
Node::Node(const NodeOptions& options)
|
||||
: options_(options),
|
||||
map_builder_(options.map_builder_options, &constant_data_),
|
||||
sensor_data_producer_(kTrajectoryBuilderId, &sensor_collator_),
|
||||
sensor_bridge_(kTrajectoryBuilderId, &sensor_collator_),
|
||||
tf_buffer_(::ros::Duration(1000)),
|
||||
tf_(tf_buffer_) {}
|
||||
|
||||
|
@ -318,7 +318,7 @@ void Node::Initialize() {
|
|||
kLaserScanTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
|
||||
[this](const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||
sensor_data_producer_.AddLaserScanMessage(kLaserScanTopic, msg);
|
||||
sensor_bridge_.AddLaserScanMessage(kLaserScanTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kLaserScanTopic);
|
||||
}
|
||||
|
@ -327,7 +327,7 @@ void Node::Initialize() {
|
|||
kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
|
||||
[this](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||
sensor_data_producer_.AddMultiEchoLaserScanMessage(
|
||||
sensor_bridge_.AddMultiEchoLaserScanMessage(
|
||||
kMultiEchoLaserScanTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kMultiEchoLaserScanTopic);
|
||||
|
@ -344,7 +344,7 @@ void Node::Initialize() {
|
|||
topic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
|
||||
[this, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
sensor_data_producer_.AddPointCloud2Message(topic, msg);
|
||||
sensor_bridge_.AddPointCloud2Message(topic, msg);
|
||||
})));
|
||||
expected_sensor_identifiers.insert(topic);
|
||||
}
|
||||
|
@ -360,7 +360,7 @@ void Node::Initialize() {
|
|||
kImuTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
|
||||
[this](const sensor_msgs::Imu::ConstPtr& msg) {
|
||||
sensor_data_producer_.AddImuMessage(kImuTopic, msg);
|
||||
sensor_bridge_.AddImuMessage(kImuTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kImuTopic);
|
||||
}
|
||||
|
@ -370,7 +370,7 @@ void Node::Initialize() {
|
|||
kOdometryTopic, kInfiniteSubscriberQueueSize,
|
||||
boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
|
||||
[this](const nav_msgs::Odometry::ConstPtr& msg) {
|
||||
sensor_data_producer_.AddOdometryMessage(kOdometryTopic, msg);
|
||||
sensor_bridge_.AddOdometryMessage(kOdometryTopic, msg);
|
||||
}));
|
||||
expected_sensor_identifiers.insert(kOdometryTopic);
|
||||
}
|
||||
|
|
|
@ -14,20 +14,20 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "sensor_data_producer.h"
|
||||
#include "sensor_bridge.h"
|
||||
|
||||
#include "msg_conversion.h"
|
||||
#include "time_conversion.h"
|
||||
|
||||
namespace cartographer_ros {
|
||||
|
||||
SensorDataProducer::SensorDataProducer(
|
||||
SensorBridge::SensorBridge(
|
||||
const int trajectory_id,
|
||||
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator)
|
||||
: trajectory_id_(trajectory_id), sensor_collator_(sensor_collator) {}
|
||||
|
||||
void SensorDataProducer::AddOdometryMessage(
|
||||
const string& topic, const nav_msgs::Odometry::ConstPtr& msg) {
|
||||
void SensorBridge::AddOdometryMessage(const string& topic,
|
||||
const nav_msgs::Odometry::ConstPtr& msg) {
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->child_frame_id,
|
||||
SensorData::Odometry{ToRigid3d(msg->pose.pose),
|
||||
|
@ -38,8 +38,8 @@ void SensorDataProducer::AddOdometryMessage(
|
|||
std::move(sensor_data));
|
||||
}
|
||||
|
||||
void SensorDataProducer::AddImuMessage(const string& topic,
|
||||
const sensor_msgs::Imu::ConstPtr& msg) {
|
||||
void SensorBridge::AddImuMessage(const string& topic,
|
||||
const sensor_msgs::Imu::ConstPtr& msg) {
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->header.frame_id, ToCartographer(*msg));
|
||||
sensor_collator_->AddSensorData(
|
||||
|
@ -48,7 +48,7 @@ void SensorDataProducer::AddImuMessage(const string& topic,
|
|||
std::move(sensor_data));
|
||||
}
|
||||
|
||||
void SensorDataProducer::AddLaserScanMessage(
|
||||
void SensorBridge::AddLaserScanMessage(
|
||||
const string& topic, const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->header.frame_id, ToCartographer(*msg));
|
||||
|
@ -58,7 +58,7 @@ void SensorDataProducer::AddLaserScanMessage(
|
|||
std::move(sensor_data));
|
||||
}
|
||||
|
||||
void SensorDataProducer::AddMultiEchoLaserScanMessage(
|
||||
void SensorBridge::AddMultiEchoLaserScanMessage(
|
||||
const string& topic, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
|
||||
auto sensor_data = ::cartographer::common::make_unique<SensorData>(
|
||||
msg->header.frame_id, ToCartographer(*msg));
|
||||
|
@ -68,7 +68,7 @@ void SensorDataProducer::AddMultiEchoLaserScanMessage(
|
|||
std::move(sensor_data));
|
||||
}
|
||||
|
||||
void SensorDataProducer::AddPointCloud2Message(
|
||||
void SensorBridge::AddPointCloud2Message(
|
||||
const string& topic, const sensor_msgs::PointCloud2::ConstPtr& msg) {
|
||||
pcl::PointCloud<pcl::PointXYZ> pcl_points;
|
||||
pcl::fromROSMsg(*msg, pcl_points);
|
|
@ -14,8 +14,8 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_SENSOR_DATA_PRODUCER_H_
|
||||
#define CARTOGRAPHER_ROS_SENSOR_DATA_PRODUCER_H_
|
||||
#ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
|
||||
#define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
|
||||
|
||||
#include "cartographer/mapping/sensor_collator.h"
|
||||
#include "geometry_msgs/Transform.h"
|
||||
|
@ -32,14 +32,14 @@ namespace cartographer_ros {
|
|||
|
||||
// A wrapper around SensorCollator that converts ROS messages into our internal
|
||||
// representation and passes them on to the 'sensor_collator'.
|
||||
class SensorDataProducer {
|
||||
class SensorBridge {
|
||||
public:
|
||||
explicit SensorDataProducer(
|
||||
explicit SensorBridge(
|
||||
int trajectory_id,
|
||||
::cartographer::mapping::SensorCollator<SensorData>* sensor_collator);
|
||||
|
||||
SensorDataProducer(const SensorDataProducer&) = delete;
|
||||
SensorDataProducer& operator=(const SensorDataProducer&) = delete;
|
||||
SensorBridge(const SensorBridge&) = delete;
|
||||
SensorBridge& operator=(const SensorBridge&) = delete;
|
||||
|
||||
void AddOdometryMessage(const string& topic,
|
||||
const nav_msgs::Odometry::ConstPtr& msg);
|
||||
|
@ -60,4 +60,4 @@ class SensorDataProducer {
|
|||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_SENSOR_DATA_PRODUCER_H_
|
||||
#endif // CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
|
Loading…
Reference in New Issue