#include <manual_control.h>
Public Member Functions | |
ManualControl () | |
Main constructor. | |
void | onInit () |
Initialize and setup controller. | |
Private Types | |
enum | { X = 0, Y, Z, K, M, N, DOF = 6, A = 6 } |
typedef navcon_msgs::ManualConfigure::Request | CRequest |
typedef navcon_msgs::ManualConfigure::Response | CResponse |
typedef std::vector< double > | DataType |
typedef boost::array< int, DOF > | GenArray |
typedef navcon_msgs::ManualSelect::Request | SRequest |
typedef navcon_msgs::ManualSelect::Response | SResponse |
Private Member Functions | |
bool | onConfiguration (CRequest &req, CResponse &resp) |
On server configuration. | |
void | onJoystick (const sensor_msgs::Joy::ConstPtr &joy) |
Handle the incoming joystick message. | |
void | onNavSts (const auv_msgs::NavSts::ConstPtr &nav) |
Handle the current navigation state. | |
bool | onSelect (SRequest &req, SResponse &resp) |
On generator selection. | |
void | pubEta (const DataType &etaff) |
Publish the Eta values. | |
void | pubNu (const DataType &nuv) |
Publish the Nu values. | |
void | pubTau (const DataType &tauv) |
Publish the Tau values. | |
void | remap (const sensor_msgs::Joy::ConstPtr &joy) |
Remap the joystick. | |
void | setDefaultConfig () |
Helper method to set the defaults values of configuration. | |
bool | setupConfig () |
Load the configuration. | |
Private Attributes | |
boost::mutex | cfg_mutex |
Config and selector mutex. | |
navcon_msgs::ManualConfiguration | config |
The node configuration. | |
ros::ServiceServer | config_server |
Configuration service server. | |
ros::Publisher | etaref |
manual reference publisher | |
GenArray | generators |
The current generators. | |
ros::Subscriber | joyin |
Joystick subscriber. | |
DataType | mapped |
The remapped joystick values. | |
boost::mutex | nav_mutex |
Nav-mutex protector. | |
bool | nav_valid |
Navigation state validity flag. | |
DataType | navstate |
The current navigation state. | |
ros::Subscriber | navsts |
Navigation state subscriber. | |
ros::Publisher | nuref |
manual reference publisher | |
ros::ServiceServer | select_server |
Selection service. | |
ros::Publisher | tauref |
manual reference publisher |
The class contains the implementation of a manual reference generator for , and . The class uses the joystick topic of ROS to remap the joystick data. The controller is driven by the joystick topic input.
The class performs joystick mappings from received axes to regular tau axes. Tau axes are defined as (X,Y,Z,K,M,N) and (-1,1). Joystick axes are defined as: (1-n) where n is the maximum axis number. The mapping is a row vector (1x6) (axes_map) that has the desired axis defined in the according place of the tau axis. For disabled axes define the axis number -1. The scaling is defined in (scale_map) as a desired double number.
Definition at line 65 of file manual_control.h.
typedef navcon_msgs::ManualConfigure::Request labust::control::ManualControl::CRequest [private] |
Definition at line 71 of file manual_control.h.
typedef navcon_msgs::ManualConfigure::Response labust::control::ManualControl::CResponse [private] |
Definition at line 72 of file manual_control.h.
typedef std::vector<double> labust::control::ManualControl::DataType [private] |
Definition at line 70 of file manual_control.h.
typedef boost::array<int,DOF> labust::control::ManualControl::GenArray [private] |
Definition at line 69 of file manual_control.h.
typedef navcon_msgs::ManualSelect::Request labust::control::ManualControl::SRequest [private] |
Definition at line 73 of file manual_control.h.
typedef navcon_msgs::ManualSelect::Response labust::control::ManualControl::SResponse [private] |
Definition at line 74 of file manual_control.h.
anonymous enum [private] |
Definition at line 67 of file manual_control.h.
Main constructor.
Definition at line 49 of file manual_control.cpp.
bool ManualControl::onConfiguration | ( | CRequest & | req, |
CResponse & | resp | ||
) | [private] |
On server configuration.
Definition at line 138 of file manual_control.cpp.
void ManualControl::onInit | ( | ) |
Initialize and setup controller.
Definition at line 58 of file manual_control.cpp.
void ManualControl::onJoystick | ( | const sensor_msgs::Joy::ConstPtr & | joy | ) | [private] |
Handle the incoming joystick message.
Definition at line 183 of file manual_control.cpp.
void ManualControl::onNavSts | ( | const auv_msgs::NavSts::ConstPtr & | nav | ) | [private] |
Handle the current navigation state.
Definition at line 220 of file manual_control.cpp.
bool ManualControl::onSelect | ( | SRequest & | req, |
SResponse & | resp | ||
) | [private] |
On generator selection.
Definition at line 168 of file manual_control.cpp.
void ManualControl::pubEta | ( | const DataType & | etaff | ) | [private] |
Publish the Eta values.
Definition at line 245 of file manual_control.cpp.
void ManualControl::pubNu | ( | const DataType & | nuv | ) | [private] |
Publish the Nu values.
Definition at line 297 of file manual_control.cpp.
void ManualControl::pubTau | ( | const DataType & | tauv | ) | [private] |
Publish the Tau values.
Definition at line 287 of file manual_control.cpp.
void ManualControl::remap | ( | const sensor_msgs::Joy::ConstPtr & | joy | ) | [private] |
Remap the joystick.
Definition at line 307 of file manual_control.cpp.
void ManualControl::setDefaultConfig | ( | ) | [private] |
Helper method to set the defaults values of configuration.
Definition at line 119 of file manual_control.cpp.
bool ManualControl::setupConfig | ( | ) | [private] |
Load the configuration.
Definition at line 82 of file manual_control.cpp.
boost::mutex labust::control::ManualControl::cfg_mutex [private] |
Config and selector mutex.
Definition at line 128 of file manual_control.h.
navcon_msgs::ManualConfiguration labust::control::ManualControl::config [private] |
The node configuration.
Definition at line 122 of file manual_control.h.
Configuration service server.
Definition at line 117 of file manual_control.h.
manual reference publisher
Definition at line 111 of file manual_control.h.
The current generators.
Definition at line 124 of file manual_control.h.
Joystick subscriber.
Definition at line 113 of file manual_control.h.
The remapped joystick values.
Definition at line 126 of file manual_control.h.
boost::mutex labust::control::ManualControl::nav_mutex [private] |
Nav-mutex protector.
Definition at line 135 of file manual_control.h.
bool labust::control::ManualControl::nav_valid [private] |
Navigation state validity flag.
Definition at line 133 of file manual_control.h.
The current navigation state.
Definition at line 131 of file manual_control.h.
Navigation state subscriber.
Definition at line 115 of file manual_control.h.
manual reference publisher
Definition at line 109 of file manual_control.h.
Selection service.
Definition at line 119 of file manual_control.h.
manual reference publisher
Definition at line 107 of file manual_control.h.