34 #ifndef MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_ACTION_H 35 #define MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_ACTION_H 44 #include <trajectory_msgs/JointTrajectory.h> 45 #include <control_msgs/FollowJointTrajectoryAction.h> 46 #include <control_msgs/FollowJointTrajectoryFeedback.h> 47 #include <industrial_msgs/RobotStatus.h> 49 #include <motoman_msgs/DynamicJointTrajectory.h> 52 namespace joint_trajectory_action
55 class JointTrajectoryAction
98 std::map<int, ros::Publisher> pub_trajectories_;
100 std::map<int, RobotGroup> robot_groups_;
108 std::map<int, ros::Subscriber> sub_trajectories_;
116 std::map<int, ros::Subscriber> sub_status_;
118 std::map<int, JointTractoryActionServer*> act_servers_;
125 std::map<int, ros::Timer>watchdog_timer_map_;
132 std::map<int, bool> has_active_goal_map_;
139 std::map<int, JointTractoryActionServer::GoalHandle> active_goal_map_;
145 std::map<int, trajectory_msgs::JointTrajectory> current_traj_map_;
147 std::vector<std::string> all_joint_names_;
176 std::map<int, control_msgs::FollowJointTrajectoryFeedbackConstPtr> last_trajectory_state_map_;
182 bool trajectory_state_recvd_;
184 std::map<int, bool> trajectory_state_recvd_map_;
194 static const double WATCHD0G_PERIOD_;
248 void controllerStateCB(
const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg);
250 void controllerStateCB(
const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
int robot_id);
260 void robotStatusCB(
const industrial_msgs::RobotStatusConstPtr &msg);
283 const trajectory_msgs::JointTrajectory & traj);
286 const trajectory_msgs::JointTrajectory & traj,
int robot_id);
292 #endif // MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_ACTION_H
void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg)
void watchdog(const ros::TimerEvent &e)
ServerGoalHandle< control_msgs::FollowJointTrajectoryAction > GoalHandle
JointTractoryActionServer action_server_
Internal action server.
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher pub_trajectory_command_
Publishes desired trajectory (typically to the robot driver)
control_msgs::FollowJointTrajectoryFeedbackConstPtr last_trajectory_state_
Cache of the last subscribed feedback message.
industrial_msgs::RobotStatusConstPtr last_robot_status_
Cache of the last subscribed status message.
bool has_active_goal_
Indicates action has an active goal.
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.
std::vector< std::string > joint_names_
The joint names associated with the robot the action is interfacing with. The joint names must be the...
ros::NodeHandle node_
Internal ROS node handle.
static const double DEFAULT_GOAL_THRESHOLD_
The default goal joint threshold see(goal_threshold). Unit are joint specific (i.e. radians or meters).
actionlib::ActionServer< control_msgs::FollowJointTrajectoryAction > JointTractoryActionServer
ros::Subscriber sub_robot_status_
Subscribes to the robot status (typically published by the robot driver).
ros::Timer watchdog_timer_
Watchdog time used to fail the action request if the robot driver is not responding.
ros::Subscriber sub_trajectory_state_
Subscribes to trajectory feedback (typically published by the robot driver).
void cancelCB(JointTractoryActionServer::GoalHandle gh)
bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, const trajectory_msgs::JointTrajectory &traj)
JointTractoryActionServer::GoalHandle active_goal_
Cache of the current active goal.
trajectory_msgs::JointTrajectory current_traj_
Cache of the current active trajectory.
void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg)
void goalCB(JointTractoryActionServer::GoalHandle gh)