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 |
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.
rsm::RobotControlMux::RobotControlMux | ( | ) |
Constructor
Definition at line 5 of file RobotControlMux.cpp.
rsm::RobotControlMux::~RobotControlMux | ( | ) |
Destructor
Definition at line 41 of file RobotControlMux.cpp.
|
private |
Callback for receiving the cmd vel produced by autonomous operation
cmd_vel | Cmd vel from autonomous operation |
Definition at line 78 of file RobotControlMux.cpp.
|
private |
Checks if the given joystick command will make the robot move and is different to the previous one
joy | Joystick commands |
Definition at line 122 of file RobotControlMux.cpp.
|
private |
Callback for receiving the commands issued from a connected joystick and checks if a new command was sent
joy | Joystick commands |
Definition at line 102 of file RobotControlMux.cpp.
|
private |
Publish the cmd vel controlling the robot depending on the current operation mode
Definition at line 50 of file RobotControlMux.cpp.
|
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.
|
private |
Definition at line 69 of file RobotControlMux.cpp.
|
private |
Callback for receiving the cmd vel from teleoperation and checks if a command not equals zero was issued
cmd_vel | Cmd vel from teleoperation |
Definition at line 83 of file RobotControlMux.cpp.
|
private |
Timer callback for checking if teleoperation was stopped and robot is idle again
event |
Definition at line 114 of file RobotControlMux.cpp.
|
private |
Last received command velocity from autonomy
Definition at line 72 of file RobotControlMux.h.
|
private |
Definition at line 42 of file RobotControlMux.h.
|
private |
Definition at line 49 of file RobotControlMux.h.
|
private |
Definition at line 44 of file RobotControlMux.h.
|
private |
Definition at line 50 of file RobotControlMux.h.
|
private |
Is emergency stop currently activated
Definition at line 64 of file RobotControlMux.h.
|
private |
Last received joystick command
Definition at line 80 of file RobotControlMux.h.
|
private |
Definition at line 43 of file RobotControlMux.h.
|
private |
Definition at line 51 of file RobotControlMux.h.
|
private |
Is a joystick in use for controlling the robot
Definition at line 68 of file RobotControlMux.h.
|
private |
Definition at line 39 of file RobotControlMux.h.
|
private |
Currently active mode of operation (0=stopped, 1=autonomous, 2=teleoperation)
Definition at line 60 of file RobotControlMux.h.
|
private |
Definition at line 45 of file RobotControlMux.h.
|
private |
Definition at line 40 of file RobotControlMux.h.
|
private |
Last received command velocity from teleoperation
Definition at line 76 of file RobotControlMux.h.
|
private |
Definition at line 41 of file RobotControlMux.h.
|
private |
Definition at line 48 of file RobotControlMux.h.
|
private |
Definition at line 46 of file RobotControlMux.h.
|
private |
Time until teleoperation mode will be stopped when no new message is received
Definition at line 56 of file RobotControlMux.h.