#include <diver_sim.h>
Public Member Functions | |
| DiverSim () | |
| The generic constructor. | |
| void | onInit () |
| Initializes the ROS node and configures the model from the ROS parameters. | |
| void | start () |
| Start the simulator. | |
Private Member Functions | |
| void | onNu (const auv_msgs::BodyVelocityReq::ConstPtr &nu) |
| Handle incoming speed request. | |
| void | step () |
| Single simulation step. | |
| void | stepJoints (double w, double hpan=0, double htilt=0) |
| Step joints. | |
Private Attributes | |
| std::vector< double > | jdefaults |
| Joint defaults. | |
| std::vector< std::string > | jnames |
| Joint names. | |
| ros::Publisher | joints |
| Joint state publisher. | |
| sensor_msgs::JointState | jstate |
| Current joint state. | |
| labust::simulation::KinematicModel | model |
| Simulated kinematics. | |
| bool | move_head |
| ros::Publisher | navsts |
| Navigation state publisher. | |
| labust::simulation::vector | nu |
| The incoming speed vector. | |
| boost::mutex | nu_mux |
| The incoming speed mutex. | |
| ros::Subscriber | nusub |
| Subscriber to commanded speeds. | |
| boost::thread | runner |
| Runner thread for the simulator. | |
| bool | runSim |
| Run flag. | |
| double | Ts |
| Sim sampling time. | |
This class implements the diver simulator. The class implements joint-state publishing for diver movement emulation and uses the kinematic model for diver movement.
Definition at line 58 of file diver_sim.h.
The generic constructor.
Definition at line 45 of file diver_sim.cpp.
| void DiverSim::onInit | ( | ) |
Initializes the ROS node and configures the model from the ROS parameters.
Definition at line 55 of file diver_sim.cpp.
| void labust::simulation::DiverSim::onNu | ( | const auv_msgs::BodyVelocityReq::ConstPtr & | nu | ) | [inline, private] |
Handle incoming speed request.
Definition at line 76 of file diver_sim.h.
| void DiverSim::start | ( | ) |
Start the simulator.
Definition at line 95 of file diver_sim.cpp.
| void DiverSim::step | ( | ) | [private] |
Single simulation step.
Definition at line 106 of file diver_sim.cpp.
| void DiverSim::stepJoints | ( | double | w, |
| double | hpan = 0, |
||
| double | htilt = 0 |
||
| ) | [private] |
Step joints.
Definition at line 148 of file diver_sim.cpp.
std::vector<double> labust::simulation::DiverSim::jdefaults [private] |
Joint defaults.
Definition at line 97 of file diver_sim.h.
std::vector<std::string> labust::simulation::DiverSim::jnames [private] |
Joint names.
Definition at line 95 of file diver_sim.h.
Joint state publisher.
Definition at line 86 of file diver_sim.h.
sensor_msgs::JointState labust::simulation::DiverSim::jstate [private] |
Current joint state.
Definition at line 99 of file diver_sim.h.
Simulated kinematics.
Definition at line 109 of file diver_sim.h.
bool labust::simulation::DiverSim::move_head [private] |
Flag to use roll/pitch rate for moving the head instead of the body directly.
Definition at line 104 of file diver_sim.h.
Navigation state publisher.
Definition at line 84 of file diver_sim.h.
The incoming speed vector.
Definition at line 90 of file diver_sim.h.
boost::mutex labust::simulation::DiverSim::nu_mux [private] |
The incoming speed mutex.
Definition at line 92 of file diver_sim.h.
Subscriber to commanded speeds.
Definition at line 88 of file diver_sim.h.
boost::thread labust::simulation::DiverSim::runner [private] |
Runner thread for the simulator.
Definition at line 112 of file diver_sim.h.
bool labust::simulation::DiverSim::runSim [private] |
Run flag.
Definition at line 114 of file diver_sim.h.
double labust::simulation::DiverSim::Ts [private] |
Sim sampling time.
Definition at line 107 of file diver_sim.h.