A class to provide velocity related functionalities. More...
#include <velocity.h>
Public Member Functions | |
geometry_msgs::Vector3 | compute_velocity (geometry_msgs::Point goal, double velocity) |
Compute the velocity vector required to reach a goal point from the current position while traveling with a given maximum velocity. More... | |
geometry_msgs::Vector3 | get_velocity () const |
Get the current velocity of the CPS. More... | |
void | move (geometry_msgs::Vector3 velocity) |
Move the CPS with a given velocity. More... | |
cpswarm_msgs::Vector | rel_velocity (geometry_msgs::Vector3 v) const |
Compute the velocity difference of the CPS to a given velocity. More... | |
velocity () | |
Constructor that initializes the private member variables. More... | |
~velocity () | |
Destructor that deletes the private member objects. More... | |
Private Member Functions | |
void | vel_callback (const geometry_msgs::TwistStamped::ConstPtr &msg) |
Callback function for velocity updates. More... | |
Private Attributes | |
geometry_msgs::Twist | current_vel |
Current velocity of the CPS. More... | |
NodeHandle | nh |
A node handle for the main ROS node. More... | |
position | pos |
A helper object for position related tasks. More... | |
Rate * | rate |
The loop rate object for running the behavior control loops at a specific frequency. More... | |
Publisher | vel_pub |
Publisher for sending the target velocity of the CPS to the velocity controller in the abstraction library. More... | |
Subscriber | vel_sub |
Subscriber for the velocity of the CPS. More... | |
bool | vel_valid |
Whether a valid velocity has been received. More... | |
A class to provide velocity related functionalities.
Definition at line 18 of file velocity.h.
velocity::velocity | ( | ) |
Constructor that initializes the private member variables.
Definition at line 3 of file velocity.cpp.
velocity::~velocity | ( | ) |
Destructor that deletes the private member objects.
Definition at line 26 of file velocity.cpp.
geometry_msgs::Vector3 velocity::compute_velocity | ( | geometry_msgs::Point | goal, |
double | velocity | ||
) |
Compute the velocity vector required to reach a goal point from the current position while traveling with a given maximum velocity.
goal | The goal coordinates. |
velocity | The velocity magnitude. |
Definition at line 31 of file velocity.cpp.
geometry_msgs::Vector3 velocity::get_velocity | ( | ) | const |
Get the current velocity of the CPS.
Definition at line 54 of file velocity.cpp.
void velocity::move | ( | geometry_msgs::Vector3 | velocity | ) |
Move the CPS with a given velocity.
/**
velocity | The velocity at which the CPS shall move. |
Definition at line 59 of file velocity.cpp.
cpswarm_msgs::Vector velocity::rel_velocity | ( | geometry_msgs::Vector3 | v | ) | const |
Compute the velocity difference of the CPS to a given velocity.
v | The velocity to compare. |
Definition at line 69 of file velocity.cpp.
|
private |
Callback function for velocity updates.
msg | Velocity received from the CPS FCU. |
Definition at line 84 of file velocity.cpp.
|
private |
Current velocity of the CPS.
Definition at line 93 of file velocity.h.
|
private |
A node handle for the main ROS node.
Definition at line 68 of file velocity.h.
|
private |
A helper object for position related tasks.
Definition at line 88 of file velocity.h.
|
private |
The loop rate object for running the behavior control loops at a specific frequency.
Definition at line 83 of file velocity.h.
|
private |
Publisher for sending the target velocity of the CPS to the velocity controller in the abstraction library.
Definition at line 78 of file velocity.h.
|
private |
Subscriber for the velocity of the CPS.
Definition at line 73 of file velocity.h.
|
private |
Whether a valid velocity has been received.
Definition at line 98 of file velocity.h.