Public Member Functions | Private Member Functions | Private Attributes | List of all members
rsm::AdditionsServiceProvider Class Reference

Class that establishes communication between the different states and the RSM's periphery including the GUI for the states and classes defined in rsm_additions package. It offers services and publishes topics based on the variables that need to be saved during state transitions. More...

#include <AdditionsServiceProvider.h>

Public Member Functions

 AdditionsServiceProvider ()
 
void publishTopics ()
 
 ~AdditionsServiceProvider ()
 

Private Member Functions

void cmdVelCallback (const geometry_msgs::Twist::ConstPtr &msg)
 
void explorationGoalCallback (const rsm_msgs::GoalStatus::ConstPtr &goal_status)
 
void explorationModeCallback (const std_msgs::Bool::ConstPtr &exploration_mode)
 
void frontierCallback (const visualization_msgs::MarkerArray::ConstPtr &frontiers)
 
bool navGoalIncludedInFrontiers ()
 
void navigationGoalCallback (const move_base_msgs::MoveBaseGoalConstPtr &frontier_goal)
 
void publishExplorationGoals ()
 
void publishFailedGoals ()
 
void publishGoalObsolete ()
 
bool resetRealsensePosition (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
 
void reverseModeCmdVelCallback (const geometry_msgs::Twist::ConstPtr &cmd_vel)
 
bool setNavigationToReverse (std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
 

Private Attributes

std::string _autonomy_cmd_vel_topic
 
bool _calculate_goal_plugin_used
 
ros::Subscriber _exploration_goal_subscriber
 
double _exploration_goal_tolerance
 
geometry_msgs::PoseArray _exploration_goals
 
ros::Publisher _exploration_goals_publisher
 
bool _exploration_mode
 
ros::Subscriber _exploration_mode_subscriber
 
geometry_msgs::PoseArray _failed_goals
 
ros::Publisher _failed_goals_publisher
 
ros::ServiceClient _get_navigation_goal_service
 
bool _goal_obsolete
 
ros::Publisher _goal_obsolete_publisher
 
ros::NodeHandle _nh
 
ros::Publisher _realsense_joint_controller
 
ros::ServiceServer _reset_realsense_position_serivce
 
ros::Publisher _reverse_mode_cmd_vel_publisher
 
ros::Subscriber _reverse_mode_cmd_vel_subscriber
 
ros::ServiceServer _set_navigation_to_reverse_service
 
MoveBaseActionServeras
 
ros::Subscriber frontiers_marker_array_subscriber
 

Detailed Description

Class that establishes communication between the different states and the RSM's periphery including the GUI for the states and classes defined in rsm_additions package. It offers services and publishes topics based on the variables that need to be saved during state transitions.

Definition at line 31 of file AdditionsServiceProvider.h.

Constructor & Destructor Documentation

◆ AdditionsServiceProvider()

rsm::AdditionsServiceProvider::AdditionsServiceProvider ( )

Definition at line 5 of file AdditionsServiceProvider.cpp.

◆ ~AdditionsServiceProvider()

rsm::AdditionsServiceProvider::~AdditionsServiceProvider ( )

Definition at line 69 of file AdditionsServiceProvider.cpp.

Member Function Documentation

◆ cmdVelCallback()

void rsm::AdditionsServiceProvider::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr &  msg)
private

Callback for receiving autonomy cmd vel messages and save the ones not equals zero in the cirular buffer

Parameters
msgCmd vel autonomy message

◆ explorationGoalCallback()

void rsm::AdditionsServiceProvider::explorationGoalCallback ( const rsm_msgs::GoalStatus::ConstPtr &  goal_status)
private

Callback for exploration goal status

Parameters
goal_statusCurrent goal status and pose

Definition at line 125 of file AdditionsServiceProvider.cpp.

◆ explorationModeCallback()

void rsm::AdditionsServiceProvider::explorationModeCallback ( const std_msgs::Bool::ConstPtr &  exploration_mode)
private

Callback for exploration mode

Parameters
exploration_modeExploration mode (0=complete goal, 1=interrupt goal when exploration goals vanished)

Definition at line 157 of file AdditionsServiceProvider.cpp.

◆ frontierCallback()

void rsm::AdditionsServiceProvider::frontierCallback ( const visualization_msgs::MarkerArray::ConstPtr &  frontiers)
private

Callback for frontier visualization generated by Explore Lite. Extracts the centers of these frontiers and saves them in a list

Parameters
frontiersMarker list containing frontiers as points and nearest frontier points for each frontier as sphere

Definition at line 134 of file AdditionsServiceProvider.cpp.

◆ navGoalIncludedInFrontiers()

bool rsm::AdditionsServiceProvider::navGoalIncludedInFrontiers ( )
private

Checks if the current navigation goal is still present as an exploration goal

Returns
Returns true if the current navigation goal is still an exploration goal to be explored

Definition at line 169 of file AdditionsServiceProvider.cpp.

◆ navigationGoalCallback()

void rsm::AdditionsServiceProvider::navigationGoalCallback ( const move_base_msgs::MoveBaseGoalConstPtr &  frontier_goal)
private

Callback for requests to the SimpleActionServer that just responds with "Goal reached"

Parameters
frontier_goal

Definition at line 102 of file AdditionsServiceProvider.cpp.

◆ publishExplorationGoals()

void rsm::AdditionsServiceProvider::publishExplorationGoals ( )
private

Publish list of extracted frontier centers for further calculation

Definition at line 107 of file AdditionsServiceProvider.cpp.

◆ publishFailedGoals()

void rsm::AdditionsServiceProvider::publishFailedGoals ( )
private

Publish list of previously failed goals

Definition at line 113 of file AdditionsServiceProvider.cpp.

◆ publishGoalObsolete()

void rsm::AdditionsServiceProvider::publishGoalObsolete ( )
private

Publish if current exploration goal is obsolete if exploration mode is set to interrupt

Definition at line 119 of file AdditionsServiceProvider.cpp.

◆ publishTopics()

void rsm::AdditionsServiceProvider::publishTopics ( )

Definition at line 73 of file AdditionsServiceProvider.cpp.

◆ resetRealsensePosition()

bool rsm::AdditionsServiceProvider::resetRealsensePosition ( std_srvs::Trigger::Request &  req,
std_srvs::Trigger::Response &  res 
)
private

Definition at line 193 of file AdditionsServiceProvider.cpp.

◆ reverseModeCmdVelCallback()

void rsm::AdditionsServiceProvider::reverseModeCmdVelCallback ( const geometry_msgs::Twist::ConstPtr &  cmd_vel)
private

Receives cmd vel and negate linear movement so robot goes backwards

Parameters
cmd_velcmd vel generated for backward movement

Definition at line 90 of file AdditionsServiceProvider.cpp.

◆ setNavigationToReverse()

bool rsm::AdditionsServiceProvider::setNavigationToReverse ( std_srvs::SetBool::Request &  req,
std_srvs::SetBool::Response &  res 
)
private

Definition at line 83 of file AdditionsServiceProvider.cpp.

Member Data Documentation

◆ _autonomy_cmd_vel_topic

std::string rsm::AdditionsServiceProvider::_autonomy_cmd_vel_topic
private

Topic name for the autonomy cmd vel topic to be recorded

Definition at line 64 of file AdditionsServiceProvider.h.

◆ _calculate_goal_plugin_used

bool rsm::AdditionsServiceProvider::_calculate_goal_plugin_used
private

Is the Calculate Goal plugin used

Definition at line 84 of file AdditionsServiceProvider.h.

◆ _exploration_goal_subscriber

ros::Subscriber rsm::AdditionsServiceProvider::_exploration_goal_subscriber
private

Definition at line 51 of file AdditionsServiceProvider.h.

◆ _exploration_goal_tolerance

double rsm::AdditionsServiceProvider::_exploration_goal_tolerance
private

Tolerance for comparing if the current goal is still in the list of exploration goals

Definition at line 76 of file AdditionsServiceProvider.h.

◆ _exploration_goals

geometry_msgs::PoseArray rsm::AdditionsServiceProvider::_exploration_goals
private

List of all extracted frontier centers

Definition at line 68 of file AdditionsServiceProvider.h.

◆ _exploration_goals_publisher

ros::Publisher rsm::AdditionsServiceProvider::_exploration_goals_publisher
private

Definition at line 48 of file AdditionsServiceProvider.h.

◆ _exploration_mode

bool rsm::AdditionsServiceProvider::_exploration_mode
private

Mode of exploration (0=complete goal, 1=interrupt goal when exploration goals vanished)

Definition at line 80 of file AdditionsServiceProvider.h.

◆ _exploration_mode_subscriber

ros::Subscriber rsm::AdditionsServiceProvider::_exploration_mode_subscriber
private

Definition at line 52 of file AdditionsServiceProvider.h.

◆ _failed_goals

geometry_msgs::PoseArray rsm::AdditionsServiceProvider::_failed_goals
private

List of previously failed goals

Definition at line 72 of file AdditionsServiceProvider.h.

◆ _failed_goals_publisher

ros::Publisher rsm::AdditionsServiceProvider::_failed_goals_publisher
private

Definition at line 45 of file AdditionsServiceProvider.h.

◆ _get_navigation_goal_service

ros::ServiceClient rsm::AdditionsServiceProvider::_get_navigation_goal_service
private

Definition at line 49 of file AdditionsServiceProvider.h.

◆ _goal_obsolete

bool rsm::AdditionsServiceProvider::_goal_obsolete
private

Is navigation goal still an exploration goal

Definition at line 88 of file AdditionsServiceProvider.h.

◆ _goal_obsolete_publisher

ros::Publisher rsm::AdditionsServiceProvider::_goal_obsolete_publisher
private

Definition at line 50 of file AdditionsServiceProvider.h.

◆ _nh

ros::NodeHandle rsm::AdditionsServiceProvider::_nh
private

Definition at line 39 of file AdditionsServiceProvider.h.

◆ _realsense_joint_controller

ros::Publisher rsm::AdditionsServiceProvider::_realsense_joint_controller
private

Definition at line 55 of file AdditionsServiceProvider.h.

◆ _reset_realsense_position_serivce

ros::ServiceServer rsm::AdditionsServiceProvider::_reset_realsense_position_serivce
private

Definition at line 54 of file AdditionsServiceProvider.h.

◆ _reverse_mode_cmd_vel_publisher

ros::Publisher rsm::AdditionsServiceProvider::_reverse_mode_cmd_vel_publisher
private

Definition at line 43 of file AdditionsServiceProvider.h.

◆ _reverse_mode_cmd_vel_subscriber

ros::Subscriber rsm::AdditionsServiceProvider::_reverse_mode_cmd_vel_subscriber
private

Definition at line 42 of file AdditionsServiceProvider.h.

◆ _set_navigation_to_reverse_service

ros::ServiceServer rsm::AdditionsServiceProvider::_set_navigation_to_reverse_service
private

Definition at line 41 of file AdditionsServiceProvider.h.

◆ as

MoveBaseActionServer* rsm::AdditionsServiceProvider::as
private

SimpleActionServer for making Explore Lite run and lead it to believe it talks to Move Base

Definition at line 60 of file AdditionsServiceProvider.h.

◆ frontiers_marker_array_subscriber

ros::Subscriber rsm::AdditionsServiceProvider::frontiers_marker_array_subscriber
private

Definition at line 47 of file AdditionsServiceProvider.h.


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


rsm_additions
Author(s): Marco Steinbrink
autogenerated on Mon Feb 28 2022 23:28:21