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

Handles all command velocities and only allows the robot to move at all if no emergency stop is set and only move autonomous if autonomous operation is active. More...

#include <RobotControlMux.h>

Public Member Functions

void publishTopics ()
 
 RobotControlMux ()
 
 ~RobotControlMux ()
 

Private Member Functions

void autonomyCmdVelCallback (const geometry_msgs::Twist::ConstPtr &cmd_vel)
 
bool checkJoystickCommand (const sensor_msgs::Joy::ConstPtr &joy)
 
void joystickCallback (const sensor_msgs::Joy::ConstPtr &joy)
 
void publishCmdVel ()
 
void publishOperationMode ()
 
bool setOperationMode (rsm_msgs::SetOperationMode::Request &req, rsm_msgs::SetOperationMode::Response &res)
 
void teleoperationCmdVelCallback (const geometry_msgs::Twist::ConstPtr &cmd_vel)
 
void teleoperationIdleTimerCallback (const ros::TimerEvent &event)
 

Private Attributes

geometry_msgs::Twist _autonomy_cmd_vel
 
ros::Subscriber _autonomy_cmd_vel_sub
 
std::string _autonomy_operation_cmd_vel_topic
 
ros::Publisher _cmd_vel_pub
 
std::string _cmd_vel_topic
 
bool _emergency_stop_active
 
sensor_msgs::Joy _joystick_cmd
 
ros::Subscriber _joystick_sub
 
std::string _joystick_topic
 
bool _joystick_used
 
ros::NodeHandle _nh
 
int _operation_mode
 
ros::Publisher _operation_mode_pub
 
ros::ServiceServer _set_operation_mode_service
 
geometry_msgs::Twist _teleoperation_cmd_vel
 
ros::Subscriber _teleoperation_cmd_vel_sub
 
std::string _teleoperation_cmd_vel_topic
 
ros::Timer _teleoperation_idle_timer
 
double _teleoperation_idle_timer_duration
 

Detailed Description

Handles all command velocities and only allows the robot to move at all if no emergency stop is set and only move autonomous if autonomous operation is active.

Definition at line 23 of file RobotControlMux.h.

Constructor & Destructor Documentation

rsm::RobotControlMux::RobotControlMux ( )

Constructor

Definition at line 5 of file RobotControlMux.cpp.

rsm::RobotControlMux::~RobotControlMux ( )

Destructor

Definition at line 41 of file RobotControlMux.cpp.

Member Function Documentation

void rsm::RobotControlMux::autonomyCmdVelCallback ( const geometry_msgs::Twist::ConstPtr &  cmd_vel)
private

Callback for receiving the cmd vel produced by autonomous operation

Parameters
cmd_velCmd vel from autonomous operation

Definition at line 78 of file RobotControlMux.cpp.

bool rsm::RobotControlMux::checkJoystickCommand ( const sensor_msgs::Joy::ConstPtr &  joy)
private

Checks if the given joystick command will make the robot move and is different to the previous one

Parameters
joyJoystick commands
Returns
If the new command will lead to a robot action

Definition at line 122 of file RobotControlMux.cpp.

void rsm::RobotControlMux::joystickCallback ( const sensor_msgs::Joy::ConstPtr &  joy)
private

Callback for receiving the commands issued from a connected joystick and checks if a new command was sent

Parameters
joyJoystick commands

Definition at line 102 of file RobotControlMux.cpp.

void rsm::RobotControlMux::publishCmdVel ( )
private

Publish the cmd vel controlling the robot depending on the current operation mode

Definition at line 50 of file RobotControlMux.cpp.

void rsm::RobotControlMux::publishOperationMode ( )
private

Publish current operation mode

Definition at line 62 of file RobotControlMux.cpp.

void rsm::RobotControlMux::publishTopics ( )

Publishes msgs from all publishers in this class (cmd vel and operation mode)

Definition at line 45 of file RobotControlMux.cpp.

bool rsm::RobotControlMux::setOperationMode ( rsm_msgs::SetOperationMode::Request &  req,
rsm_msgs::SetOperationMode::Response &  res 
)
private

Definition at line 69 of file RobotControlMux.cpp.

void rsm::RobotControlMux::teleoperationCmdVelCallback ( const geometry_msgs::Twist::ConstPtr &  cmd_vel)
private

Callback for receiving the cmd vel from teleoperation and checks if a command not equals zero was issued

Parameters
cmd_velCmd vel from teleoperation

Definition at line 83 of file RobotControlMux.cpp.

void rsm::RobotControlMux::teleoperationIdleTimerCallback ( const ros::TimerEvent event)
private

Timer callback for checking if teleoperation was stopped and robot is idle again

Parameters
event

Definition at line 114 of file RobotControlMux.cpp.

Member Data Documentation

geometry_msgs::Twist rsm::RobotControlMux::_autonomy_cmd_vel
private

Last received command velocity from autonomy

Definition at line 72 of file RobotControlMux.h.

ros::Subscriber rsm::RobotControlMux::_autonomy_cmd_vel_sub
private

Definition at line 42 of file RobotControlMux.h.

std::string rsm::RobotControlMux::_autonomy_operation_cmd_vel_topic
private

Definition at line 49 of file RobotControlMux.h.

ros::Publisher rsm::RobotControlMux::_cmd_vel_pub
private

Definition at line 44 of file RobotControlMux.h.

std::string rsm::RobotControlMux::_cmd_vel_topic
private

Definition at line 50 of file RobotControlMux.h.

bool rsm::RobotControlMux::_emergency_stop_active
private

Is emergency stop currently activated

Definition at line 64 of file RobotControlMux.h.

sensor_msgs::Joy rsm::RobotControlMux::_joystick_cmd
private

Last received joystick command

Definition at line 80 of file RobotControlMux.h.

ros::Subscriber rsm::RobotControlMux::_joystick_sub
private

Definition at line 43 of file RobotControlMux.h.

std::string rsm::RobotControlMux::_joystick_topic
private

Definition at line 51 of file RobotControlMux.h.

bool rsm::RobotControlMux::_joystick_used
private

Is a joystick in use for controlling the robot

Definition at line 68 of file RobotControlMux.h.

ros::NodeHandle rsm::RobotControlMux::_nh
private

Definition at line 39 of file RobotControlMux.h.

int rsm::RobotControlMux::_operation_mode
private

Currently active mode of operation (0=stopped, 1=autonomous, 2=teleoperation)

Definition at line 60 of file RobotControlMux.h.

ros::Publisher rsm::RobotControlMux::_operation_mode_pub
private

Definition at line 45 of file RobotControlMux.h.

ros::ServiceServer rsm::RobotControlMux::_set_operation_mode_service
private

Definition at line 40 of file RobotControlMux.h.

geometry_msgs::Twist rsm::RobotControlMux::_teleoperation_cmd_vel
private

Last received command velocity from teleoperation

Definition at line 76 of file RobotControlMux.h.

ros::Subscriber rsm::RobotControlMux::_teleoperation_cmd_vel_sub
private

Definition at line 41 of file RobotControlMux.h.

std::string rsm::RobotControlMux::_teleoperation_cmd_vel_topic
private

Definition at line 48 of file RobotControlMux.h.

ros::Timer rsm::RobotControlMux::_teleoperation_idle_timer
private

Definition at line 46 of file RobotControlMux.h.

double rsm::RobotControlMux::_teleoperation_idle_timer_duration
private

Time until teleoperation mode will be stopped when no new message is received

Definition at line 56 of file RobotControlMux.h.


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


rsm_core
Author(s): Marco Steinbrink
autogenerated on Tue Mar 16 2021 02:44:31