#include <joint_trajectory_action.h>
Public Member Functions | |
JointTrajectoryAction () | |
Constructor. | |
void | run () |
Begin processing messages and publishing topics. | |
~JointTrajectoryAction () | |
Destructor. | |
Private Types | |
typedef actionlib::ActionServer < control_msgs::FollowJointTrajectoryAction > | JointTractoryActionServer |
Private Member Functions | |
void | abortGoal () |
Aborts the current action goal and sends a stop command (empty message) to the robot driver. | |
void | cancelCB (JointTractoryActionServer::GoalHandle &gh) |
Action server cancel callback method. | |
void | controllerStateCB (const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg) |
Controller state callback (executed when feedback message received) | |
void | goalCB (JointTractoryActionServer::GoalHandle &gh) |
Action server goal callback method. | |
void | robotStatusCB (const industrial_msgs::RobotStatusConstPtr &msg) |
Controller status callback (executed when robot status message received) | |
void | watchdog (const ros::TimerEvent &e) |
Watch dog callback, used to detect robot driver failures. | |
bool | withinGoalConstraints (const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, const trajectory_msgs::JointTrajectory &traj) |
Controller status callback (executed when robot status message received) | |
Private Attributes | |
JointTractoryActionServer | action_server_ |
Internal action server. | |
JointTractoryActionServer::GoalHandle | active_goal_ |
Cache of the current active goal. | |
bool | controller_alive_ |
Controller was alive during the last watchdog interval. | |
trajectory_msgs::JointTrajectory | current_traj_ |
Cache of the current active trajectory. | |
double | goal_threshold_ |
The goal joint threshold used for determining if a robot is near it final destination. A single value is used for all joints. | |
bool | has_active_goal_ |
Indicates action has an active goal. | |
bool | has_moved_once_ |
Indicates that the robot has been in a moving state at least once since starting the current active trajectory. | |
std::vector< std::string > | joint_names_ |
The joint names associated with the robot the action is interfacing with. The joint names must be the same as expected by the robot driver. | |
industrial_msgs::RobotStatusConstPtr | last_robot_status_ |
Cache of the last subscribed status message. | |
control_msgs::FollowJointTrajectoryFeedbackConstPtr | last_trajectory_state_ |
Cache of the last subscribed feedback message. | |
ros::NodeHandle | node_ |
Internal ROS node handle. | |
ros::Publisher | pub_trajectory_command_ |
Publishes desired trajectory (typically to the robot driver) | |
ros::Subscriber | sub_robot_status_ |
Subscribes to the robot status (typically published by the robot driver). | |
ros::Subscriber | sub_trajectory_state_ |
Subscribes to trajectory feedback (typically published by the robot driver). | |
ros::Time | time_to_check_ |
Time at which to start checking for completion of current goal, if one is active. | |
ros::Timer | watchdog_timer_ |
Watchdog time used to fail the action request if the robot driver is not responding. | |
Static Private Attributes | |
static const double | DEFAULT_GOAL_THRESHOLD_ = 0.01 |
The default goal joint threshold see(goal_threshold). Unit are joint specific (i.e. radians or meters). | |
static const double | WATCHDOG_PERIOD_ = 1.0 |
The watchdog period (seconds) |
Definition at line 48 of file joint_trajectory_action.h.
typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::JointTractoryActionServer [private] |
Definition at line 71 of file joint_trajectory_action.h.
Constructor.
Definition at line 45 of file joint_trajectory_action.cpp.
Destructor.
Definition at line 70 of file joint_trajectory_action.cpp.
void industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::abortGoal | ( | ) | [private] |
Aborts the current action goal and sends a stop command (empty message) to the robot driver.
Definition at line 275 of file joint_trajectory_action.cpp.
void industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::cancelCB | ( | JointTractoryActionServer::GoalHandle & | gh | ) | [private] |
Action server cancel callback method.
gh | goal handle |
Definition at line 183 of file joint_trajectory_action.cpp.
void industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::controllerStateCB | ( | const control_msgs::FollowJointTrajectoryFeedbackConstPtr & | msg | ) | [private] |
Controller state callback (executed when feedback message received)
msg | joint trajectory feedback message |
Definition at line 203 of file joint_trajectory_action.cpp.
void industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::goalCB | ( | JointTractoryActionServer::GoalHandle & | gh | ) | [private] |
Action server goal callback method.
gh | goal handle |
Definition at line 110 of file joint_trajectory_action.cpp.
void industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::robotStatusCB | ( | const industrial_msgs::RobotStatusConstPtr & | msg | ) | [private] |
Controller status callback (executed when robot status message received)
msg | robot status message |
Definition at line 74 of file joint_trajectory_action.cpp.
Begin processing messages and publishing topics.
Definition at line 67 of file joint_trajectory_action.h.
void industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::watchdog | ( | const ros::TimerEvent & | e | ) | [private] |
Watch dog callback, used to detect robot driver failures.
e | time event information |
Definition at line 80 of file joint_trajectory_action.cpp.
bool industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::withinGoalConstraints | ( | const control_msgs::FollowJointTrajectoryFeedbackConstPtr & | msg, |
const trajectory_msgs::JointTrajectory & | traj | ||
) | [private] |
Controller status callback (executed when robot status message received)
msg | trajectory feedback message |
traj | trajectory to test against feedback |
Definition at line 286 of file joint_trajectory_action.cpp.
JointTractoryActionServer industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::action_server_ [private] |
Internal action server.
Definition at line 81 of file joint_trajectory_action.h.
JointTractoryActionServer::GoalHandle industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::active_goal_ [private] |
Cache of the current active goal.
Definition at line 125 of file joint_trajectory_action.h.
bool industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::controller_alive_ [private] |
Controller was alive during the last watchdog interval.
Definition at line 109 of file joint_trajectory_action.h.
trajectory_msgs::JointTrajectory industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::current_traj_ [private] |
Cache of the current active trajectory.
Definition at line 129 of file joint_trajectory_action.h.
const double industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01 [static, private] |
The default goal joint threshold see(goal_threshold). Unit are joint specific (i.e. radians or meters).
Definition at line 135 of file joint_trajectory_action.h.
double industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::goal_threshold_ [private] |
The goal joint threshold used for determining if a robot is near it final destination. A single value is used for all joints.
NOTE: This value is used in conjunction with the robot inMotion status (see industrial_msgs::RobotStatus) if it exists.
Definition at line 144 of file joint_trajectory_action.h.
bool industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::has_active_goal_ [private] |
Indicates action has an active goal.
Definition at line 114 of file joint_trajectory_action.h.
bool industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::has_moved_once_ [private] |
Indicates that the robot has been in a moving state at least once since starting the current active trajectory.
Definition at line 120 of file joint_trajectory_action.h.
std::vector<std::string> industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::joint_names_ [private] |
The joint names associated with the robot the action is interfacing with. The joint names must be the same as expected by the robot driver.
Definition at line 151 of file joint_trajectory_action.h.
industrial_msgs::RobotStatusConstPtr industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::last_robot_status_ [private] |
Cache of the last subscribed status message.
Definition at line 161 of file joint_trajectory_action.h.
control_msgs::FollowJointTrajectoryFeedbackConstPtr industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::last_trajectory_state_ [private] |
Cache of the last subscribed feedback message.
Definition at line 156 of file joint_trajectory_action.h.
ros::NodeHandle industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::node_ [private] |
Internal ROS node handle.
Definition at line 76 of file joint_trajectory_action.h.
ros::Publisher industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::pub_trajectory_command_ [private] |
Publishes desired trajectory (typically to the robot driver)
Definition at line 86 of file joint_trajectory_action.h.
ros::Subscriber industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::sub_robot_status_ [private] |
Subscribes to the robot status (typically published by the robot driver).
Definition at line 98 of file joint_trajectory_action.h.
ros::Subscriber industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::sub_trajectory_state_ [private] |
Subscribes to trajectory feedback (typically published by the robot driver).
Definition at line 92 of file joint_trajectory_action.h.
ros::Time industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::time_to_check_ [private] |
Time at which to start checking for completion of current goal, if one is active.
Definition at line 167 of file joint_trajectory_action.h.
const double industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::WATCHDOG_PERIOD_ = 1.0 [static, private] |
The watchdog period (seconds)
Definition at line 172 of file joint_trajectory_action.h.
ros::Timer industrial_robot_client::joint_trajectory_action::JointTrajectoryAction::watchdog_timer_ [private] |
Watchdog time used to fail the action request if the robot driver is not responding.
Definition at line 104 of file joint_trajectory_action.h.