Hekateros follow joint trajectory action server class. More...
#include <hekateros_as_follow_traj.h>
Public Member Functions | |
ASFollowTrajectory (std::string name, HekaterosControl &node) | |
Initialization constructor. | |
void | execute_cb (const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) |
ROS callback to execute action. | |
void | preempt_cb () |
ROS callback to preempt action. | |
void | start () |
Start the action server. | |
virtual | ~ASFollowTrajectory () |
Destructor. | |
Static Public Attributes | |
static const int | MaxIters = 10 |
maximum iterations w/o motion | |
static const double | TolGoal = 0.00087 |
final goal position | |
static const double | TolWaypoint = 0.01745 |
intermediate waypoint position | |
Protected Member Functions | |
bool | atWaypoint (trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint) |
Test if current move is at the target waypoint. | |
bool | failedWaypoint (trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint) |
Test if current move to the the target waypoint failed. | |
void | monitorMove (trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint) |
Monitor and provide feedback of current waypoint move. | |
int | moveToWaypoint (trajectory_msgs::JointTrajectory &jt, ssize_t iWaypoint) |
Move to next waypoint. | |
void | publishFeedback (ssize_t iWaypoint) |
Publish feedback. | |
Protected Attributes | |
std::string | action_name_ |
action name | |
actionlib::SimpleActionServer < control_msgs::FollowJointTrajectoryAction > | as_ |
action simple server | |
control_msgs::FollowJointTrajectoryFeedback | feedback_ |
progress feedback | |
double | m_fTolerance |
waypoint tolerance | |
double | m_fWaypointDist |
current waypoint distance | |
int | m_iterMonitor |
monitoring iteration count | |
HekaterosControl & | node_ |
hekateros control node instance | |
control_msgs::FollowJointTrajectoryResult | result_ |
action results |
Hekateros follow joint trajectory action server class.
Definition at line 103 of file hekateros_as_follow_traj.h.
hekateros_control::ASFollowTrajectory::ASFollowTrajectory | ( | std::string | name, |
HekaterosControl & | node | ||
) | [inline] |
Initialization constructor.
name | Action server name. |
node | Node-specific class instance. |
Definition at line 117 of file hekateros_as_follow_traj.h.
virtual hekateros_control::ASFollowTrajectory::~ASFollowTrajectory | ( | ) | [inline, virtual] |
Destructor.
Definition at line 139 of file hekateros_as_follow_traj.h.
bool hekateros_control::ASFollowTrajectory::atWaypoint | ( | trajectory_msgs::JointTrajectory & | jt, |
ssize_t | iWaypoint | ||
) | [inline, protected] |
Test if current move is at the target waypoint.
jt | Joint trajectory path. |
iWaypoint | Current waypoint along the trajectoy path. |
Definition at line 210 of file hekateros_as_follow_traj.h.
void ASFollowTrajectory::execute_cb | ( | const control_msgs::FollowJointTrajectoryGoalConstPtr & | goal | ) |
ROS callback to execute action.
The callback is executed in a separate ROS-created thread so that it can block.Typically, the callback is invoked within a ROS spin.
goal | Goal joint trajectory path. |
Definition at line 139 of file hekateros_as_follow_traj.cpp.
bool ASFollowTrajectory::failedWaypoint | ( | trajectory_msgs::JointTrajectory & | jt, |
ssize_t | iWaypoint | ||
) | [protected] |
Test if current move to the the target waypoint failed.
jt | Joint trajectory path. |
iWaypoint | Current waypoint along the trajectoy path. |
Definition at line 379 of file hekateros_as_follow_traj.cpp.
void ASFollowTrajectory::monitorMove | ( | trajectory_msgs::JointTrajectory & | jt, |
ssize_t | iWaypoint | ||
) | [protected] |
Monitor and provide feedback of current waypoint move.
jt | Joint trajectory path. |
iWaypoint | Current waypoint along the trajectoy path. |
Definition at line 292 of file hekateros_as_follow_traj.cpp.
int ASFollowTrajectory::moveToWaypoint | ( | trajectory_msgs::JointTrajectory & | jt, |
ssize_t | iWaypoint | ||
) | [protected] |
Move to next waypoint.
jt | Joint trajectory path. |
iWaypoint | Current waypoint along the trajectoy path. |
Definition at line 265 of file hekateros_as_follow_traj.cpp.
void ASFollowTrajectory::preempt_cb | ( | ) |
ROS callback to preempt action.
This is only needed if actions are required outside of the blocking execution callback thread.
Definition at line 420 of file hekateros_as_follow_traj.cpp.
void ASFollowTrajectory::publishFeedback | ( | ssize_t | iWaypoint | ) | [protected] |
Publish feedback.
iWaypoint | Current waypoint along the trajectoy path. |
Definition at line 404 of file hekateros_as_follow_traj.cpp.
void hekateros_control::ASFollowTrajectory::start | ( | void | ) | [inline] |
Start the action server.
Definition at line 146 of file hekateros_as_follow_traj.h.
std::string hekateros_control::ASFollowTrajectory::action_name_ [protected] |
action name
Definition at line 171 of file hekateros_as_follow_traj.h.
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> hekateros_control::ASFollowTrajectory::as_ [protected] |
action simple server
Definition at line 174 of file hekateros_as_follow_traj.h.
control_msgs::FollowJointTrajectoryFeedback hekateros_control::ASFollowTrajectory::feedback_ [protected] |
progress feedback
Definition at line 176 of file hekateros_as_follow_traj.h.
double hekateros_control::ASFollowTrajectory::m_fTolerance [protected] |
waypoint tolerance
Definition at line 180 of file hekateros_as_follow_traj.h.
double hekateros_control::ASFollowTrajectory::m_fWaypointDist [protected] |
current waypoint distance
Definition at line 181 of file hekateros_as_follow_traj.h.
int hekateros_control::ASFollowTrajectory::m_iterMonitor [protected] |
monitoring iteration count
Definition at line 182 of file hekateros_as_follow_traj.h.
const int ASFollowTrajectory::MaxIters = 10 [static] |
maximum iterations w/o motion
Definition at line 109 of file hekateros_as_follow_traj.h.
hekateros control node instance
Definition at line 172 of file hekateros_as_follow_traj.h.
control_msgs::FollowJointTrajectoryResult hekateros_control::ASFollowTrajectory::result_ [protected] |
action results
Definition at line 178 of file hekateros_as_follow_traj.h.
const double ASFollowTrajectory::TolGoal = 0.00087 [static] |
final goal position
Definition at line 108 of file hekateros_as_follow_traj.h.
const double ASFollowTrajectory::TolWaypoint = 0.01745 [static] |
intermediate waypoint position
Definition at line 107 of file hekateros_as_follow_traj.h.