#include <usv_gazebo_thrust_plugin.h>
Public Member Functions | |
virtual void | Load (physics::ModelPtr _parent, sdf::ElementPtr _sdf) |
UsvThrust () | |
virtual | ~UsvThrust () |
Protected Member Functions | |
virtual void | FiniChild () |
virtual void | UpdateChild () |
Private Member Functions | |
double | getSdfParamDouble (sdf::ElementPtr sdfPtr, const std::string ¶m_name, double default_val) |
double | glf (double x, float A, float K, float B, float v, float C, float M) |
double | glfThrustCmd (double cmd) |
void | OnCmdDrive (const usv_gazebo_plugins::UsvDriveConstPtr &msg) |
double | scaleThrustCmd (double cmd) |
void | spin () |
Private Attributes | |
ros::Subscriber | cmd_drive_sub_ |
double | cmd_timeout_ |
double | last_cmd_drive_left_ |
double | last_cmd_drive_right_ |
common::Time | last_cmd_drive_time_ |
physics::LinkPtr | link_ |
std::string | link_name_ |
physics::ModelPtr | model_ |
std::string | node_namespace_ |
Parameters. | |
double | param_boat_length_ |
double | param_boat_width_ |
int | param_mapping_type_ |
double | param_max_cmd_ |
double | param_max_force_fwd_ |
double | param_max_force_rev_ |
double | param_thrust_z_offset_ |
math::Pose | pose_ |
common::Time | prev_update_time_ |
ros::NodeHandle * | rosnode_ |
boost::thread * | spinner_thread_ |
event::ConnectionPtr | update_connection_ |
event::ConnectionPtr | updateConnection |
physics::WorldPtr | world_ |
Definition at line 45 of file usv_gazebo_thrust_plugin.h.
Definition at line 30 of file usv_gazebo_thrust_plugin.cpp.
UsvThrust::~UsvThrust | ( | ) | [virtual] |
Definition at line 34 of file usv_gazebo_thrust_plugin.cpp.
void UsvThrust::FiniChild | ( | ) | [protected, virtual] |
Definition at line 42 of file usv_gazebo_thrust_plugin.cpp.
double UsvThrust::getSdfParamDouble | ( | sdf::ElementPtr | sdfPtr, |
const std::string & | param_name, | ||
double | default_val | ||
) | [private] |
Convenience function for getting SDF parameters
Definition at line 47 of file usv_gazebo_thrust_plugin.cpp.
double UsvThrust::glf | ( | double | x, |
float | A, | ||
float | K, | ||
float | B, | ||
float | v, | ||
float | C, | ||
float | M | ||
) | [private] |
Definition at line 177 of file usv_gazebo_thrust_plugin.cpp.
double UsvThrust::glfThrustCmd | ( | double | cmd | ) | [private] |
Definition at line 182 of file usv_gazebo_thrust_plugin.cpp.
void UsvThrust::Load | ( | physics::ModelPtr | _parent, |
sdf::ElementPtr | _sdf | ||
) | [virtual] |
Loads the model in gets dynamic parameters from SDF.
Definition at line 62 of file usv_gazebo_thrust_plugin.cpp.
void UsvThrust::OnCmdDrive | ( | const usv_gazebo_plugins::UsvDriveConstPtr & | msg | ) | [private] |
Callback for Drive commands
msg | usv_msgs UsvDrive message |
Definition at line 258 of file usv_gazebo_thrust_plugin.cpp.
double UsvThrust::scaleThrustCmd | ( | double | cmd | ) | [private] |
Takes ROS Kingfisher Drive commands and scales them by max thrust
cmd | ROS drive command |
max_cmd | Maximum value expected for commands - scaling factor |
max_pos | Maximum positive force value |
max_neg | Maximum negative force value |
Definition at line 161 of file usv_gazebo_thrust_plugin.cpp.
void UsvThrust::spin | ( | ) | [private] |
ROS spin once
Definition at line 266 of file usv_gazebo_thrust_plugin.cpp.
void UsvThrust::UpdateChild | ( | ) | [protected, virtual] |
Callback for Gazebo simulation engine
Definition at line 202 of file usv_gazebo_thrust_plugin.cpp.
Definition at line 92 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::cmd_timeout_ [private] |
Timeout for recieving Drive commands [s]
Definition at line 109 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::last_cmd_drive_left_ [private] |
Definition at line 112 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::last_cmd_drive_right_ [private] |
Definition at line 113 of file usv_gazebo_thrust_plugin.h.
common::Time gazebo::UsvThrust::last_cmd_drive_time_ [private] |
Definition at line 111 of file usv_gazebo_thrust_plugin.h.
physics::LinkPtr gazebo::UsvThrust::link_ [private] |
Pointer to model link in gazebo, optionally specified by the bodyName parameter, The states are taken from this link and forces applied to this link.
Definition at line 106 of file usv_gazebo_thrust_plugin.h.
std::string gazebo::UsvThrust::link_name_ [private] |
Definition at line 88 of file usv_gazebo_thrust_plugin.h.
physics::ModelPtr gazebo::UsvThrust::model_ [private] |
Pointer to Gazebo parent model, retrieved when the model is loaded
Definition at line 102 of file usv_gazebo_thrust_plugin.h.
std::string gazebo::UsvThrust::node_namespace_ [private] |
Parameters.
Definition at line 87 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::param_boat_length_ [private] |
Plugin Parameter: Boat length [m]
Definition at line 127 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::param_boat_width_ [private] |
Plugin Parameter: Boat width [m]
Definition at line 125 of file usv_gazebo_thrust_plugin.h.
int gazebo::UsvThrust::param_mapping_type_ [private] |
Definition at line 116 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::param_max_cmd_ [private] |
Plugin Parameter: Maximum (abs val) of Drive commands. typ. +/-1.0
Definition at line 118 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::param_max_force_fwd_ [private] |
Plugin Parameter: Maximum forward force [N]
Definition at line 120 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::param_max_force_rev_ [private] |
Plugin Parameter: Maximum reverse force [N]
Definition at line 122 of file usv_gazebo_thrust_plugin.h.
double gazebo::UsvThrust::param_thrust_z_offset_ [private] |
Plugin Parameter: Z offset for applying forward thrust
Definition at line 129 of file usv_gazebo_thrust_plugin.h.
math::Pose gazebo::UsvThrust::pose_ [private] |
Definition at line 107 of file usv_gazebo_thrust_plugin.h.
common::Time gazebo::UsvThrust::prev_update_time_ [private] |
Definition at line 110 of file usv_gazebo_thrust_plugin.h.
ros::NodeHandle* gazebo::UsvThrust::rosnode_ [private] |
Definition at line 90 of file usv_gazebo_thrust_plugin.h.
boost::thread* gazebo::UsvThrust::spinner_thread_ [private] |
Definition at line 97 of file usv_gazebo_thrust_plugin.h.
Definition at line 96 of file usv_gazebo_thrust_plugin.h.
Definition at line 131 of file usv_gazebo_thrust_plugin.h.
physics::WorldPtr gazebo::UsvThrust::world_ [private] |
Pointer to the Gazebo world, retrieved when the model is loaded
Definition at line 100 of file usv_gazebo_thrust_plugin.h.