Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate > Class Template Referenceabstract

An interface for publishing NavGridOfX/OccupancyGrid msgs and their updates periodically. More...

#include <nav_grid_publisher.h>

Public Member Functions

 GenericGridPublisher (nav_grid::NavGrid< NumericType > &data)
 
void init (ros::NodeHandle &nh, const std::string &nav_grid_topic="grid", const std::string &occupancy_grid_topic="costmap", const std::string &update_area_topic="update_area", bool publish_updates=true, ros::Duration full_publish_cycle=ros::Duration(0), ros::Duration update_publish_cycle=ros::Duration(0))
 Initialize method for determining what gets published when. More...
 
void publish ()
 Publish the full grid if the full_publish_cycle allows. More...
 
void publish (const nav_core2::UIntBounds &bounds)
 Publish the full grid or updates, as dictated by parameters passed to init. More...
 

Protected Member Functions

template<class FullGridType , class UpdateType , class Callback >
void createPublishers (ros::NodeHandle &nh, const std::string &topic, Callback new_subscription_callback, ros::Publisher &full_grid_pub, ros::Publisher &update_pub, bool publish_updates)
 
void onNewSubscriptionNav (const ros::SingleSubscriberPublisher &pub)
 
void onNewSubscriptionOcc (const ros::SingleSubscriberPublisher &pub)
 
void publishNav ()
 
void publishNavUpdate (const nav_core2::UIntBounds &bounds)
 
void publishOcc ()
 
void publishOccUpdate (const nav_core2::UIntBounds &bounds)
 
void publishUpdateArea (const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds)
 
bool shouldPublishFull () const
 
bool shouldPublishHelper (const ros::Time &last_publish, const ros::Duration &cycle) const
 
bool shouldPublishUpdate () const
 
virtual nav_msgs::OccupancyGrid toOccupancyGrid (const ros::Time &timestamp)=0
 
virtual map_msgs::OccupancyGridUpdate toOccupancyGridUpdate (const nav_core2::UIntBounds &bounds, const ros::Time &timestamp)=0
 

Protected Attributes

nav_grid::NavGrid< NumericType > & data_
 
ros::Duration full_publish_cycle_
 
ros::Time last_full_publish_
 
ros::Time last_update_publish_
 
ros::Publisher nav_pub_
 
ros::Publisher nav_update_pub_
 
ros::Publisher occ_pub_
 
ros::Publisher occ_update_pub_
 
bool publish_updates_
 
nav_grid::NavGridInfo saved_info_
 
ros::Time synced_time_stamp_
 
ros::Publisher update_area_pub_
 
nav_core2::UIntBounds update_bounds_
 
ros::Duration update_publish_cycle_
 

Detailed Description

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
class nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >

An interface for publishing NavGridOfX/OccupancyGrid msgs and their updates periodically.

This class can potentially publish on five different messages on five topics

The main reason for the two types of publishers (NavGridOfX vs OccupancyGrid) is that the standard usage for OccupancyGrid has been that values are ranged [0, 100] with -1/255 reserved for unknown. Implementing classes need to find a way to convert the full range of values to fit that range. NavGridOfChars does not follow that convention, so all values are used, and NavGridOfDoubles does not need to limit to the 256 values of OccupancyGrid. However, there are legacy applications that like the OccupancyGrid message, so we maintain the ability to publish both.

Full Grid vs. Update Publishing: If you just call publish() with no params, this class will only publish the full grid messages (NavGridOfX/OccupancyGrid). If you call publish(...) with the bounds parameter, this class will publish the full grid messages if publish_updates was set to false during init or the NavGridInfo has changed (i.e. the size/origin/resolution/frame has changed). Otherwise, it will publish the update messages (NavGridOfXUpdate/OccupancyGridUpdate) and the update_area polygon.

How often the full grid is published is controlled by the full_publish_cycle and how often you call one of the publish methods.

Definition at line 106 of file nav_grid_publisher.h.

Constructor & Destructor Documentation

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::GenericGridPublisher ( nav_grid::NavGrid< NumericType > &  data)
inlineexplicit

Definition at line 109 of file nav_grid_publisher.h.

Member Function Documentation

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
template<class FullGridType , class UpdateType , class Callback >
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::createPublishers ( ros::NodeHandle nh,
const std::string &  topic,
Callback  new_subscription_callback,
ros::Publisher full_grid_pub,
ros::Publisher update_pub,
bool  publish_updates 
)
inlineprotected

Definition at line 214 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::init ( ros::NodeHandle nh,
const std::string &  nav_grid_topic = "grid",
const std::string &  occupancy_grid_topic = "costmap",
const std::string &  update_area_topic = "update_area",
bool  publish_updates = true,
ros::Duration  full_publish_cycle = ros::Duration(0),
ros::Duration  update_publish_cycle = ros::Duration(0) 
)
inline

Initialize method for determining what gets published when.

Parameters
nhNodeHandle used for creating publishers
gridData source
nav_grid_topicTopic to publish the NavGridOfX on. If empty, don't publish that message.
occupancy_grid_topicTopic to publish the OccupancyGrid on. If empty, don't publish that message.
update_area_topicTopic to publish the update area polygon on. If empty, don't publish that message.
publish_updatesIf true, publishes _update topics. If false, always publish full grid.
full_publish_cycleIf positive, limits how often the full grid is published. If negative, never publishes.
update_publish_cycleIf positive, limits how often the update is published. If negative, never publishes.

Note: We use the callback functions for when the publisher gets a new subscription to ensure the new subscriber gets the most up to date data. If we just latched the topic, several updates may have occurred between the first whole grid publication and the new subscription, so it would have out-of-date information.

Definition at line 127 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::onNewSubscriptionNav ( const ros::SingleSubscriberPublisher pub)
inlineprotected

Definition at line 276 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::onNewSubscriptionOcc ( const ros::SingleSubscriberPublisher pub)
inlineprotected

Definition at line 281 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publish ( )
inline

Publish the full grid if the full_publish_cycle allows.

Definition at line 158 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publish ( const nav_core2::UIntBounds bounds)
inline

Publish the full grid or updates, as dictated by parameters passed to init.

The bounds provided are of the form [min, max] i.e. the maximum is included in the range. If either range is empty, that indicates none of the grid has been updated, but we still may want to publish some of the data.

Definition at line 174 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publishNav ( )
inlineprotected

Definition at line 286 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publishNavUpdate ( const nav_core2::UIntBounds bounds)
inlineprotected

Definition at line 298 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publishOcc ( )
inlineprotected

Definition at line 292 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publishOccUpdate ( const nav_core2::UIntBounds bounds)
inlineprotected

Definition at line 304 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
void nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publishUpdateArea ( const nav_grid::NavGridInfo info,
const nav_core2::UIntBounds bounds 
)
inlineprotected

Definition at line 258 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
bool nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::shouldPublishFull ( ) const
inlineprotected

Definition at line 248 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
bool nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::shouldPublishHelper ( const ros::Time last_publish,
const ros::Duration cycle 
) const
inlineprotected

Definition at line 231 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
bool nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::shouldPublishUpdate ( ) const
inlineprotected

Definition at line 253 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
virtual nav_msgs::OccupancyGrid nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::toOccupancyGrid ( const ros::Time timestamp)
protectedpure virtual
template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
virtual map_msgs::OccupancyGridUpdate nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::toOccupancyGridUpdate ( const nav_core2::UIntBounds bounds,
const ros::Time timestamp 
)
protectedpure virtual

Member Data Documentation

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
nav_grid::NavGrid<NumericType>& nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::data_
protected

Definition at line 311 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Duration nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::full_publish_cycle_
protected

Definition at line 316 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Time nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::last_full_publish_
protected

Definition at line 317 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Time nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::last_update_publish_
protected

Definition at line 317 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Publisher nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::nav_pub_
protected

Definition at line 323 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Publisher nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::nav_update_pub_
protected

Definition at line 323 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Publisher nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::occ_pub_
protected

Definition at line 323 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Publisher nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::occ_update_pub_
protected

Definition at line 323 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
bool nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::publish_updates_
protected

Definition at line 313 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
nav_grid::NavGridInfo nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::saved_info_
protected

Definition at line 312 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Time nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::synced_time_stamp_
protected

Definition at line 317 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Publisher nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::update_area_pub_
protected

Definition at line 323 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
nav_core2::UIntBounds nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::update_bounds_
protected

Definition at line 320 of file nav_grid_publisher.h.

template<typename NumericType, typename NavGridOfX, typename NavGridOfXUpdate>
ros::Duration nav_grid_pub_sub::GenericGridPublisher< NumericType, NavGridOfX, NavGridOfXUpdate >::update_publish_cycle_
protected

Definition at line 316 of file nav_grid_publisher.h.


The documentation for this class was generated from the following file:


nav_grid_pub_sub
Author(s):
autogenerated on Sun Jan 10 2021 04:08:50