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

Configurable occupancy grid publishing speed (#504)

master
Jihoon Lee 2017-10-05 15:40:45 +02:00 committed by Holger Rapp
parent e02e634848
commit a2c8a8fe2d
1 changed files with 19 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -38,6 +38,7 @@
DEFINE_double(resolution, 0.05, DEFINE_double(resolution, 0.05,
"Resolution of a grid cell in the published occupancy grid."); "Resolution of a grid cell in the published occupancy grid.");
DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
namespace cartographer_ros { namespace cartographer_ros {
namespace { namespace {
@ -97,7 +98,7 @@ void CairoDrawEachSubmap(
class Node { class Node {
public: public:
explicit Node(double resolution); explicit Node(double resolution, double publish_period_sec);
~Node() {} ~Node() {}
Node(const Node&) = delete; Node(const Node&) = delete;
@ -105,7 +106,7 @@ class Node {
private: private:
void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
void DrawAndPublish(const string& frame_id, const ros::Time& time); void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
void PublishOccupancyGrid(const string& frame_id, const ros::Time& time, void PublishOccupancyGrid(const string& frame_id, const ros::Time& time,
const Eigen::Array2f& origin, const Eigen::Array2f& origin,
const Eigen::Array2i& size, const Eigen::Array2i& size,
@ -119,9 +120,12 @@ class Node {
::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
std::map<SubmapId, SubmapState> submaps_ GUARDED_BY(mutex_); std::map<SubmapId, SubmapState> submaps_ GUARDED_BY(mutex_);
::ros::WallTimer occupancy_grid_publisher_timer_;
std::string last_frame_id_;
ros::Time last_timestamp_;
}; };
Node::Node(const double resolution) Node::Node(const double resolution, const double publish_period_sec)
: resolution_(resolution), : resolution_(resolution),
client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
kSubmapQueryServiceName)), kSubmapQueryServiceName)),
@ -135,9 +139,10 @@ Node::Node(const double resolution)
occupancy_grid_publisher_( occupancy_grid_publisher_(
node_handle_.advertise<::nav_msgs::OccupancyGrid>( node_handle_.advertise<::nav_msgs::OccupancyGrid>(
kOccupancyGridTopic, kLatestOnlyPublisherQueueSize, kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
true /* latched */)) true /* latched */)),
occupancy_grid_publisher_timer_(
{} node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
&Node::DrawAndPublish, this)) {}
void Node::HandleSubmapList( void Node::HandleSubmapList(
const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
@ -193,14 +198,16 @@ void Node::HandleSubmapList(
<< cairo_status_to_string( << cairo_status_to_string(
cairo_surface_status(submap_state.surface.get())); cairo_surface_status(submap_state.surface.get()));
} }
DrawAndPublish(msg->header.frame_id, msg->header.stamp); last_timestamp_ = msg->header.stamp;
last_frame_id_ = msg->header.frame_id;
} }
void Node::DrawAndPublish(const string& frame_id, const ros::Time& time) { void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
if (submaps_.empty()) { if (submaps_.empty() || last_frame_id_.empty()) {
return; return;
} }
::cartographer::common::MutexLocker locker(&mutex_);
Eigen::AlignedBox2f bounding_box; Eigen::AlignedBox2f bounding_box;
{ {
auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr( auto surface = ::cartographer::io::MakeUniqueCairoSurfacePtr(
@ -244,7 +251,8 @@ void Node::DrawAndPublish(const string& frame_id, const ros::Time& time) {
cairo_paint(cr.get()); cairo_paint(cr.get());
}); });
cairo_surface_flush(surface.get()); cairo_surface_flush(surface.get());
PublishOccupancyGrid(frame_id, time, origin, size, surface.get()); PublishOccupancyGrid(last_frame_id_, last_timestamp_, origin, size,
surface.get());
} }
} }
@ -299,7 +307,7 @@ int main(int argc, char** argv) {
::ros::start(); ::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink; cartographer_ros::ScopedRosLogSink ros_log_sink;
::cartographer_ros::Node node(FLAGS_resolution); ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec);
::ros::spin(); ::ros::spin();
::ros::shutdown(); ::ros::shutdown();