diff --git a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h index 277b23f3..6cc256db 100644 --- a/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h +++ b/motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_action.h @@ -47,6 +47,7 @@ #include #include #include +#include namespace industrial_robot_client { namespace joint_trajectory_action @@ -95,8 +96,6 @@ class JointTrajectoryAction */ ros::Publisher pub_trajectory_command_; - std::map pub_trajectories_; - std::map robot_groups_; /** @@ -105,45 +104,50 @@ class JointTrajectoryAction */ ros::Subscriber sub_trajectory_state_; - std::map sub_trajectories_; - /** * \brief Subscribes to the robot status (typically published by the * robot driver). */ ros::Subscriber sub_robot_status_; - std::map sub_status_; + /** + * \brief Subscribes to motoros error messages to abort trajectories. + */ + ros::Subscriber sub_motoros_errors_; - std::map act_servers_; /** * \brief Watchdog time used to fail the action request if the robot * driver is not responding. */ ros::Timer watchdog_timer_; - std::mapwatchdog_timer_map_; + /** + * \brief The time that the active trajectory is expected to end execution. + * Used to determine when to begin checking for goal completion. + */ + ros::Time expected_traj_end_; /** * \brief Indicates action has an active goal */ bool has_active_goal_; - std::map has_active_goal_map_; + /** + * \brief Indicates which groups are active on `multi-group` action goals, + * Note: independant of `has_active_goal_map_` which handles single group goals. + */ + std::map has_active_goals_map_; /** * \brief Cache of the current active goal */ JointTractoryActionServer::GoalHandle active_goal_; - std::map active_goal_map_; /** * \brief Cache of the current active trajectory */ trajectory_msgs::JointTrajectory current_traj_; - std::map current_traj_map_; - std::vector all_joint_names_; /** @@ -161,13 +165,6 @@ class JointTrajectoryAction */ double goal_threshold_; - /** - * \brief 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. - */ - std::vector joint_names_; - /** * \brief Cache of the last subscribed feedback message */ @@ -179,8 +176,6 @@ class JointTrajectoryAction * \brief Indicates trajectory state has been received. Used by * watchdog to determine if the robot driver is responding. */ - bool trajectory_state_recvd_; - std::map trajectory_state_recvd_map_; /** @@ -191,7 +186,7 @@ class JointTrajectoryAction /** * \brief The watchdog period (seconds) */ - static const double WATCHD0G_PERIOD_; // = 1.0; + static const double WATCHDOG_PERIOD_; // = 1.0; /** * \brief Watch dog callback, used to detect robot driver failures @@ -201,8 +196,6 @@ class JointTrajectoryAction */ void watchdog(const ros::TimerEvent &e); - void watchdog(const ros::TimerEvent &e, int group_number); - /** * \brief Action server goal callback method * @@ -220,6 +213,7 @@ class JointTrajectoryAction */ void cancelCB(JointTractoryActionServer::GoalHandle gh); + /** * \brief Controller state callback (executed when feedback message * received) @@ -227,37 +221,31 @@ class JointTrajectoryAction * \param msg joint trajectory feedback message * */ - - void goalCB(JointTractoryActionServer::GoalHandle gh, int group_number); + void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg); /** - * \brief Action server cancel callback method + * \brief Controller status callback (executed when robot status + * message received) * - * \param gh goal handle + * \param msg robot status message * */ + void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg); - void cancelCB(JointTractoryActionServer::GoalHandle gh, int group_number); /** - * \brief Controller state callback (executed when feedback message - * received) + * \brief Motoross error callback (executed when motoros error + * message received) * - * \param msg joint trajectory feedback message + * \param msg Motoros error message * */ - void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg); - - void controllerStateCB(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, int robot_id); - + void motorosErrorCB(const motoman_msgs::MotorosError &msg); /** - * \brief Controller status callback (executed when robot status - * message received) - * - * \param msg robot status message + * \brief Marks the current action goal as succeeded. * */ - void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg); + void setGoalSuccess(); /** * \brief Aborts the current action goal and sends a stop command @@ -267,7 +255,12 @@ class JointTrajectoryAction */ void abortGoal(); - void abortGoal(int robot_id); + /** + * \brief Cancels the current action goal and sends a stop command + * (empty message) to the robot driver. + * + */ + void cancelGoal(); /** * \brief Controller status callback (executed when robot status @@ -279,15 +272,10 @@ class JointTrajectoryAction * \return true if all joints are within goal contraints * */ - bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, - const trajectory_msgs::JointTrajectory & traj); - - bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, - const trajectory_msgs::JointTrajectory & traj, int robot_id); + bool withinGoalConstraints(const trajectory_msgs::JointTrajectory & traj); }; } // namespace joint_trajectory_action } // namespace industrial_robot_client #endif // MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_ACTION_H - diff --git a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h index 1e53cf0a..5232c851 100644 --- a/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h +++ b/motoman_driver/include/motoman_driver/joint_trajectory_streamer.h @@ -132,6 +132,11 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer virtual void streamingThread(); + /** + * \brief Safely disable Yaskawa arm before system shutdown. + */ + void shutdown(); + protected: int robot_id_; MotomanMotionCtrl motion_ctrl_; @@ -168,6 +173,12 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer */ ros::ServiceServer srv_select_tool_; + /** + * \brief Publisher to announce motoros failures. This allows higher-level + * drivers to retry certain actions. + */ + ros::Publisher motoros_error_pub_; + /** * \brief Disable the robot. Response is true if the state was flipped or * false if the state has not changed. diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp index e9b80ac3..3ca1c8cd 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_action.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include using industrial_robot_client::motoman_utils::getJointGroups; @@ -47,7 +48,7 @@ namespace industrial_robot_client namespace joint_trajectory_action { -const double JointTrajectoryAction::WATCHD0G_PERIOD_ = 1.0; +const double JointTrajectoryAction::WATCHDOG_PERIOD_ = 1.0; const double JointTrajectoryAction::DEFAULT_GOAL_THRESHOLD_ = 0.01; JointTrajectoryAction::JointTrajectoryAction() : @@ -59,60 +60,37 @@ JointTrajectoryAction::JointTrajectoryAction() : pn.param("constraints/goal_threshold", goal_threshold_, DEFAULT_GOAL_THRESHOLD_); - std::map robot_groups; - if (!getJointGroups("topic_list", robot_groups)) - { - // this is a WARN as this class is the multi-group version of the regular JTA, - // and we're actually expecting to find the 'topic_list' parameter, as using - // this multi-group version with a single-group system is unnecessary and also - // doesn't make much sense. - // It probably also won't work. - ROS_WARN("Expecting/assuming single motion-group controller configuration"); - } + getJointGroups("topic_list", robot_groups_); - for (size_t i = 0; i < robot_groups.size(); i++) + for (size_t i = 0; i < robot_groups_.size(); i++) { - std::string joint_path_action_name = robot_groups[i].get_ns() + "/" + robot_groups[i].get_name(); - std::vector rg_joint_names = robot_groups[i].get_joint_names(); - int group_number_int = robot_groups[i].get_group_id(); + std::vector rg_joint_names = robot_groups_[i].get_joint_names(); + int group_number_int = robot_groups_[i].get_group_id(); all_joint_names_.insert(all_joint_names_.end(), rg_joint_names.begin(), rg_joint_names.end()); - actionServer_ = new actionlib::ActionServer( - node_, joint_path_action_name + "/joint_trajectory_action" , false); - actionServer_->registerGoalCallback( - boost::bind(&JointTrajectoryAction::goalCB, - this, _1, group_number_int)); - actionServer_->registerCancelCallback( - boost::bind(&JointTrajectoryAction::cancelCB, - this, _1, group_number_int)); - - pub_trajectory_command_ = node_.advertise( - joint_path_action_name + "/joint_path_command", 1); - sub_trajectory_state_ = this->node_.subscribe( - joint_path_action_name + "/feedback_states", 1, - boost::bind(&JointTrajectoryAction::controllerStateCB, - this, _1, group_number_int)); - sub_robot_status_ = node_.subscribe( - "robot_status", 1, &JointTrajectoryAction::robotStatusCB, this); - - pub_trajectories_[group_number_int] = pub_trajectory_command_; - sub_trajectories_[group_number_int] = (sub_trajectory_state_); - sub_status_[group_number_int] = (sub_robot_status_); - - this->act_servers_[group_number_int] = actionServer_; - - this->act_servers_[group_number_int]->start(); - - this->watchdog_timer_map_[group_number_int] = node_.createTimer( - ros::Duration(WATCHD0G_PERIOD_), boost::bind( - &JointTrajectoryAction::watchdog, this, _1, group_number_int)); + // Trajectories sub-group active map initialization + has_active_goals_map_[group_number_int] = false; + // Trajectory state recvd init + trajectory_state_recvd_map_[group_number_int] = false; } pub_trajectory_command_ = node_.advertise( "joint_path_command", 1); - this->robot_groups_ = robot_groups; + sub_trajectory_state_ = node_.subscribe( + "feedback_states", 1, + boost::bind(&JointTrajectoryAction::controllerStateCB, this, _1)); + + sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this); + + sub_motoros_errors_ = node_.subscribe("motoros_error", 1, &JointTrajectoryAction::motorosErrorCB, this); + + // Set watchdog timer for entire robot state. + watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), + boost::bind(&JointTrajectoryAction::watchdog, this, _1)); + + has_active_goal_ = false; action_server_.start(); } @@ -134,240 +112,123 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e) { ROS_DEBUG("Waiting for subscription to joint trajectory state"); } - if (!trajectory_state_recvd_) + bool trajectory_state_recvd = true; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) { - ROS_DEBUG("Trajectory state not received since last watchdog"); + trajectory_state_recvd &= trajectory_state_recvd_map_[group_number] | !has_active_goals_map_[group_number]; } - // Aborts the active goal if the controller does not appear to be active. - if (has_active_goal_) + if (!trajectory_state_recvd) { - if (!trajectory_state_recvd_) + ROS_DEBUG("Trajectory state of the entire robot not received since last watchdog"); + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) { - // last_trajectory_state_ is null if the subscriber never makes a connection - if (!last_trajectory_state_) - { - ROS_WARN("Aborting goal because we have never heard a controller state message."); - } - else + if (!trajectory_state_recvd_map_[group_number]) { - ROS_WARN_STREAM( - "Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ << " seconds"); + ROS_DEBUG("Group [%s] state not received since last watchdog", + robot_groups_[group_number].get_name().c_str()); } - abortGoal(); } } - // Reset the trajectory state received flag - trajectory_state_recvd_ = false; -} - -void JointTrajectoryAction::watchdog(const ros::TimerEvent &e, int group_number) -{ - // Some debug logging - if (!last_trajectory_state_map_[group_number]) - { - ROS_DEBUG("Waiting for subscription to joint trajectory state"); - } - if (!trajectory_state_recvd_map_[group_number]) - { - ROS_DEBUG("Trajectory state not received since last watchdog"); - } - // Aborts the active goal if the controller does not appear to be active. - if (has_active_goal_map_[group_number]) + if (has_active_goal_) { - if (!trajectory_state_recvd_map_[group_number]) + if (!trajectory_state_recvd) { // last_trajectory_state_ is null if the subscriber never makes a connection - if (!last_trajectory_state_map_[group_number]) + if (!last_trajectory_state_) { ROS_WARN("Aborting goal because we have never heard a controller state message."); } else { ROS_WARN_STREAM( - "Aborting goal because we haven't heard from the controller in " << WATCHD0G_PERIOD_ << " seconds"); + "Aborting goal because we haven't heard from the controller in " << WATCHDOG_PERIOD_ << " seconds"); } - abortGoal(group_number); + abortGoal(); } } - // Reset the trajectory state received flag - trajectory_state_recvd_ = false; + + // Reset the multi-group trajectory state received flag + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) + { + trajectory_state_recvd_map_[group_number] = false; + } } void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh) { - gh.setAccepted(); - - int group_number; - -// TODO(thiagodefreitas): change for getting the id from the group instead of a sequential checking on the map + ROS_DEBUG("Multi-group trajectory execution request received"); - ros::Duration last_time_from_start(0.0); - - motoman_msgs::DynamicJointTrajectory dyn_traj; - - for (size_t i = 0; i < gh.getGoal()->trajectory.points.size(); i++) + if (!gh.getGoal()->trajectory.points.empty()) { - motoman_msgs::DynamicJointPoint dpoint; + std::map group_joints_start_idx; + ros::Duration last_time_from_start(0.0); - for (size_t rbt_idx = 0; rbt_idx < robot_groups_.size(); rbt_idx++) + if (has_active_goal_) // Cancels the currently active goal. + { + ROS_WARN("Received new goal, canceling current goal"); + cancelGoal(); + } + // Detect which robot groups are active in the current motion trajectory + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) { size_t ros_idx = std::find( - gh.getGoal()->trajectory.joint_names.begin(), - gh.getGoal()->trajectory.joint_names.end(), - robot_groups_[rbt_idx].get_joint_names()[0]) - - gh.getGoal()->trajectory.joint_names.begin(); + gh.getGoal()->trajectory.joint_names.begin(), + gh.getGoal()->trajectory.joint_names.end(), + robot_groups_[group_number].get_joint_names()[0]) + - gh.getGoal()->trajectory.joint_names.begin(); bool is_found = ros_idx < gh.getGoal()->trajectory.joint_names.size(); - - group_number = rbt_idx; - motoman_msgs::DynamicJointsGroup dyn_group; - - int num_joints = robot_groups_[group_number].get_joint_names().size(); - if (is_found) { - if (gh.getGoal()->trajectory.points[i].positions.empty()) - { - std::vector positions(num_joints, 0.0); - dyn_group.positions = positions; - } - else - dyn_group.positions.insert( - dyn_group.positions.begin(), - gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx, - gh.getGoal()->trajectory.points[i].positions.begin() + ros_idx - + robot_groups_[rbt_idx].get_joint_names().size()); - - if (gh.getGoal()->trajectory.points[i].velocities.empty()) - { - std::vector velocities(num_joints, 0.0); - dyn_group.velocities = velocities; - } - else - dyn_group.velocities.insert( - dyn_group.velocities.begin(), - gh.getGoal()->trajectory.points[i].velocities.begin() - + ros_idx, gh.getGoal()->trajectory.points[i].velocities.begin() - + ros_idx + robot_groups_[rbt_idx].get_joint_names().size()); - - if (gh.getGoal()->trajectory.points[i].accelerations.empty()) - { - std::vector accelerations(num_joints, 0.0); - dyn_group.accelerations = accelerations; - } - else - dyn_group.accelerations.insert( - dyn_group.accelerations.begin(), - gh.getGoal()->trajectory.points[i].accelerations.begin() - + ros_idx, gh.getGoal()->trajectory.points[i].accelerations.begin() - + ros_idx + robot_groups_[rbt_idx].get_joint_names().size()); - if (gh.getGoal()->trajectory.points[i].effort.empty()) - { - std::vector effort(num_joints, 0.0); - dyn_group.effort = effort; - } - else - dyn_group.effort.insert( - dyn_group.effort.begin(), - gh.getGoal()->trajectory.points[i].effort.begin() - + ros_idx, gh.getGoal()->trajectory.points[i].effort.begin() - + ros_idx + robot_groups_[rbt_idx].get_joint_names().size()); - dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start; - dyn_group.group_number = group_number; - dyn_group.num_joints = dyn_group.positions.size(); + has_active_goals_map_[group_number] = true; + group_joints_start_idx.insert(std::pair(group_number, ros_idx)); + ROS_DEBUG("Group [%s] is active in trajectory plan", robot_groups_[group_number].get_name().c_str()); } - - // Generating message for groups that were not present in the trajectory message else { - std::vector positions(num_joints, 0.0); - std::vector velocities(num_joints, 0.0); - std::vector accelerations(num_joints, 0.0); - std::vector effort(num_joints, 0.0); - - dyn_group.positions = positions; - dyn_group.velocities = velocities; - dyn_group.accelerations = accelerations; - dyn_group.effort = effort; - - dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start; - dyn_group.group_number = group_number; - dyn_group.num_joints = num_joints; + has_active_goals_map_[group_number] = false; + ROS_DEBUG("Group [%s] not present in trajectory plan, using its last received joint positions as goal", + robot_groups_[group_number].get_name().c_str()); } - - dpoint.groups.push_back(dyn_group); } - dpoint.num_groups = dpoint.groups.size(); - dyn_traj.points.push_back(dpoint); - } - dyn_traj.header = gh.getGoal()->trajectory.header; - dyn_traj.header.stamp = ros::Time::now(); - // Publishing the joint names for the 4 groups - dyn_traj.joint_names = all_joint_names_; - - this->pub_trajectory_command_.publish(dyn_traj); -} + gh.setAccepted(); + active_goal_ = gh; + has_active_goal_ = true; + motoman_msgs::DynamicJointTrajectory dyn_traj; -void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle gh) -{ - // The interface is provided, but it is recommended to use - // void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle & gh, int group_number) + ROS_DEBUG("Publishing trajectory"); - ROS_DEBUG("Received action cancel request"); -} + current_traj_ = active_goal_.getGoal()->trajectory; -void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh, int group_number) -{ - if (!gh.getGoal()->trajectory.points.empty()) - { - if (industrial_utils::isSimilar( - this->robot_groups_[group_number].get_joint_names(), - gh.getGoal()->trajectory.joint_names)) + for (int i = 0; i < gh.getGoal()->trajectory.points.size(); i++) { - // Cancels the currently active goal. - if (has_active_goal_map_[group_number]) - { - ROS_WARN("Received new goal, canceling current goal"); - abortGoal(group_number); - } - // Sends the trajectory along to the controller - if (withinGoalConstraints(last_trajectory_state_map_[group_number], - gh.getGoal()->trajectory, group_number)) - { - ROS_INFO_STREAM("Already within goal constraints, setting goal succeeded"); - gh.setAccepted(); - gh.setSucceeded(); - has_active_goal_map_[group_number] = false; - } - else - { - gh.setAccepted(); - active_goal_map_[group_number] = gh; - has_active_goal_map_[group_number] = true; - - ROS_INFO("Publishing trajectory"); + motoman_msgs::DynamicJointPoint dpoint; - current_traj_map_[group_number] = active_goal_map_[group_number].getGoal()->trajectory; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) + { + motoman_msgs::DynamicJointsGroup dyn_group; int num_joints = robot_groups_[group_number].get_joint_names().size(); - motoman_msgs::DynamicJointTrajectory dyn_traj; - - for (size_t i = 0; i < current_traj_map_[group_number].points.size(); ++i) + if (has_active_goals_map_[group_number]) // If group is active on current goal { - motoman_msgs::DynamicJointsGroup dyn_group; - motoman_msgs::DynamicJointPoint dyn_point; - if (gh.getGoal()->trajectory.points[i].positions.empty()) { std::vector positions(num_joints, 0.0); dyn_group.positions = positions; } else - dyn_group.positions = gh.getGoal()->trajectory.points[i].positions; + { + // This assumes joints in the same group are sequential and in the group-defined order. + dyn_group.positions.insert( + dyn_group.positions.begin(), + gh.getGoal()->trajectory.points[i].positions.begin() + group_joints_start_idx[group_number], + gh.getGoal()->trajectory.points[i].positions.begin() + group_joints_start_idx[group_number] + + robot_groups_[group_number].get_joint_names().size()); + } if (gh.getGoal()->trajectory.points[i].velocities.empty()) { @@ -375,55 +236,92 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh, int dyn_group.velocities = velocities; } else - dyn_group.velocities = gh.getGoal()->trajectory.points[i].velocities; + { + dyn_group.velocities.insert( + dyn_group.velocities.begin(), + gh.getGoal()->trajectory.points[i].velocities.begin() + group_joints_start_idx[group_number], + gh.getGoal()->trajectory.points[i].velocities.begin() + group_joints_start_idx[group_number] + + robot_groups_[group_number].get_joint_names().size()); + } + if (gh.getGoal()->trajectory.points[i].accelerations.empty()) { std::vector accelerations(num_joints, 0.0); dyn_group.accelerations = accelerations; } else - dyn_group.accelerations = gh.getGoal()->trajectory.points[i].accelerations; + { + dyn_group.accelerations.insert( + dyn_group.accelerations.begin(), + gh.getGoal()->trajectory.points[i].accelerations.begin() + group_joints_start_idx[group_number], + gh.getGoal()->trajectory.points[i].accelerations.begin() + group_joints_start_idx[group_number] + + robot_groups_[group_number].get_joint_names().size()); + } + if (gh.getGoal()->trajectory.points[i].effort.empty()) { std::vector effort(num_joints, 0.0); dyn_group.effort = effort; } else - dyn_group.effort = gh.getGoal()->trajectory.points[i].effort; + { + dyn_group.effort.insert( + dyn_group.effort.begin(), + gh.getGoal()->trajectory.points[i].effort.begin() + group_joints_start_idx[group_number], + gh.getGoal()->trajectory.points[i].effort.begin() + group_joints_start_idx[group_number] + + robot_groups_[group_number].get_joint_names().size()); + } + dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start; dyn_group.group_number = group_number; - dyn_group.num_joints = robot_groups_[group_number].get_joint_names().size(); - dyn_point.groups.push_back(dyn_group); + dyn_group.num_joints = dyn_group.positions.size(); + } + + // Generating message for groups that were not present in the trajectory message + // Assume that joints from these groups will mantain its current position. + // Velocity, acceleration and effort are zero out. + else + { + std::vector positions = last_trajectory_state_map_[group_number]->actual.positions; + std::vector velocities(num_joints, 0.0); + std::vector accelerations(num_joints, 0.0); + std::vector effort(num_joints, 0.0); - dyn_point.num_groups = 1; - dyn_traj.points.push_back(dyn_point); + dyn_group.positions = positions; + dyn_group.velocities = velocities; + dyn_group.accelerations = accelerations; + dyn_group.effort = effort; + + dyn_group.time_from_start = gh.getGoal()->trajectory.points[i].time_from_start; + dyn_group.group_number = group_number; + dyn_group.num_joints = num_joints; } - dyn_traj.header = gh.getGoal()->trajectory.header; - dyn_traj.joint_names = gh.getGoal()->trajectory.joint_names; - this->pub_trajectories_[group_number].publish(dyn_traj); + + dpoint.groups.push_back(dyn_group); } + + dpoint.num_groups = dpoint.groups.size(); + dyn_traj.points.push_back(dpoint); } - else - { - ROS_ERROR("Joint trajectory action failing on invalid joints"); - control_msgs::FollowJointTrajectoryResult rslt; - rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS; - gh.setRejected(rslt, "Joint names do not match"); - } + + dyn_traj.header = gh.getGoal()->trajectory.header; + dyn_traj.header.stamp = ros::Time::now(); + // Publishing the joint names for the 4 groups + dyn_traj.joint_names = all_joint_names_; + + this->pub_trajectory_command_.publish(dyn_traj); + + expected_traj_end_ = ros::Time::now() + gh.getGoal()->trajectory.points.back().time_from_start; } else { - ROS_ERROR("Joint trajectory action failed on empty trajectory"); + ROS_ERROR("Multi-group joint trajectory action failed on empty trajectory"); control_msgs::FollowJointTrajectoryResult rslt; rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; gh.setRejected(rslt, "Empty trajectory"); } // Adding some informational log messages to indicate unsupported goal constraints - if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0) - { - ROS_WARN_STREAM("Ignoring goal time tolerance in action goal, may be supported in the future"); - } if (!gh.getGoal()->goal_tolerance.empty()) { ROS_WARN_STREAM( @@ -435,20 +333,12 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle gh, int } } -void JointTrajectoryAction::cancelCB( - JointTractoryActionServer::GoalHandle gh, int group_number) +void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle gh) { ROS_DEBUG("Received action cancel request"); - if (active_goal_map_[group_number] == gh) + if (has_active_goal_ && active_goal_ == gh) { - // Stops the controller. - motoman_msgs::DynamicJointTrajectory empty; - empty.joint_names = robot_groups_[group_number].get_joint_names(); - this->pub_trajectories_[group_number].publish(empty); - - // Marks the current goal as canceled. - active_goal_map_[group_number].setCanceled(); - has_active_goal_map_[group_number] = false; + cancelGoal(); } else { @@ -457,91 +347,63 @@ void JointTrajectoryAction::cancelCB( } void JointTrajectoryAction::controllerStateCB( - const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, int robot_id) + const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg) { ROS_DEBUG("Checking controller state feedback"); - last_trajectory_state_map_[robot_id] = msg; - trajectory_state_recvd_map_[robot_id] = true; - if (!has_active_goal_map_[robot_id]) + // Full robot state is marked as received if all robot groups states have been received. + int msg_group = -1; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) { - ROS_DEBUG("No active goal, ignoring feedback"); - return; - } + size_t idx = std::find(msg->joint_names.begin(), + msg->joint_names.end(), + robot_groups_[group_number].get_joint_names()[0]) + - msg->joint_names.begin(); - if (current_traj_map_[robot_id].points.empty()) - { - ROS_DEBUG("Current trajectory is empty, ignoring feedback"); - return; + bool is_found = idx < msg->joint_names.size(); + if (is_found) + { + msg_group = group_number; + break; + } } - if (!industrial_utils::isSimilar(robot_groups_[robot_id].get_joint_names(), msg->joint_names)) + if (msg_group == -1) { - ROS_ERROR("Joint names from the controller don't match our joint names."); + ROS_WARN("Unrecognized controller feedback message"); return; } - // Checking for goal constraints - // Checks that we have ended inside the goal constraints and has motion stopped + last_trajectory_state_ = msg; + trajectory_state_recvd_map_[msg_group] = true; + last_trajectory_state_map_[msg_group] = msg; - ROS_DEBUG("Checking goal constraints"); - if (withinGoalConstraints(last_trajectory_state_map_[robot_id], current_traj_map_[robot_id], robot_id)) + bool trajectory_state_recvd = true; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) { - if (last_robot_status_) - { - // Additional check for motion stoppage since the controller goal may still - // be moving. The current robot driver calls a motion stop if it receives - // a new trajectory while it is still moving. If the driver is not publishing - // the motion state (i.e. old driver), this will still work, but it warns you. - if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE) - { - ROS_INFO("Inside goal constraints, stopped moving, return success for action"); - active_goal_map_[robot_id].setSucceeded(); - has_active_goal_map_[robot_id] = false; - } - else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN) - { - ROS_INFO("Inside goal constraints, return success for action"); - ROS_WARN("Robot status in motion unknown, the robot driver node and controller code should be updated"); - active_goal_map_[robot_id].setSucceeded(); - has_active_goal_map_[robot_id] = false; - } - else - { - ROS_DEBUG("Within goal constraints but robot is still moving"); - } - } - else - { - ROS_INFO("Inside goal constraints, return success for action"); - ROS_WARN("Robot status is not being published the robot driver node and controller code should be updated"); - active_goal_map_[robot_id].setSucceeded(); - has_active_goal_map_[robot_id] = false; - } + trajectory_state_recvd &= trajectory_state_recvd_map_[group_number] | !has_active_goals_map_[group_number]; } -} - -void JointTrajectoryAction::controllerStateCB( - const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg) -{ - ROS_DEBUG("Checking controller state feedback"); - last_trajectory_state_ = msg; - trajectory_state_recvd_ = true; + if (!trajectory_state_recvd) + { + ROS_DEBUG("Waiting for all robot groups feedback states before processing multi-group feedback"); + return; + } if (!has_active_goal_) { - ROS_DEBUG("No active goal, ignoring feedback"); + ROS_DEBUG("No active multi-group goal, ignoring feedback"); return; } + if (current_traj_.points.empty()) { ROS_DEBUG("Current trajectory is empty, ignoring feedback"); return; } - if (!industrial_utils::isSimilar(all_joint_names_, msg->joint_names)) + if (ros::Time::now() < expected_traj_end_) { - ROS_ERROR("Joint names from the controller don't match our joint names."); + ROS_DEBUG("Motion has not ended, ignoring feedback"); return; } @@ -549,26 +411,25 @@ void JointTrajectoryAction::controllerStateCB( // Checks that we have ended inside the goal constraints and has motion stopped ROS_DEBUG("Checking goal constraints"); - if (withinGoalConstraints(last_trajectory_state_, current_traj_)) + if (withinGoalConstraints(current_traj_)) { if (last_robot_status_) { // Additional check for motion stoppage since the controller goal may still - // be moving. The current robot driver calls a motion stop if it receives + // be moving. The current robot driver calls a motion stop if it receivesjoint_traj_client // a new trajectory while it is still moving. If the driver is not publishing // the motion state (i.e. old driver), this will still work, but it warns you. if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE) { ROS_INFO("Inside goal constraints, stopped moving, return success for action"); - active_goal_.setSucceeded(); - has_active_goal_ = false; + setGoalSuccess(); } else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::UNKNOWN) { + // TODO(NA): Should this really return succeeded? ROS_INFO("Inside goal constraints, return success for action"); ROS_WARN("Robot status in motion unknown, the robot driver node and controller code should be updated"); - active_goal_.setSucceeded(); - has_active_goal_ = false; + setGoalSuccess(); } else { @@ -579,95 +440,137 @@ void JointTrajectoryAction::controllerStateCB( { ROS_INFO("Inside goal constraints, return success for action"); ROS_WARN("Robot status is not being published the robot driver node and controller code should be updated"); - active_goal_.setSucceeded(); - has_active_goal_ = false; + setGoalSuccess(); } } + else if (last_robot_status_->in_motion.val == industrial_msgs::TriState::FALSE) + { + ROS_WARN("Outside goal constraints, aborting trajectory"); + abortGoal(); + } +} + +void JointTrajectoryAction::motorosErrorCB(const motoman_msgs::MotorosError &msg) +{ + ROS_WARN_STREAM( + "Encountered motoros error " << msg.code << "-" << msg.subcode << ": " + << msg.code_description << " " << msg.subcode_description); + if (has_active_goal_) + { + abortGoal(); + } +} + +void JointTrajectoryAction::setGoalSuccess() +{ + ROS_INFO("Marking goal as successful."); + + // Marks the current goal as aborted. + active_goal_.setSucceeded(); + has_active_goal_ = false; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) + { + has_active_goals_map_[group_number] = false; + } } void JointTrajectoryAction::abortGoal() { + ROS_INFO("Aborting active goal."); // Stops the controller. - trajectory_msgs::JointTrajectory empty; + motoman_msgs::DynamicJointTrajectory empty; pub_trajectory_command_.publish(empty); // Marks the current goal as aborted. active_goal_.setAborted(); has_active_goal_ = false; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) + { + has_active_goals_map_[group_number] = false; + } } -void JointTrajectoryAction::abortGoal(int robot_id) +void JointTrajectoryAction::cancelGoal() { + ROS_INFO("Cancelling active goal."); // Stops the controller. motoman_msgs::DynamicJointTrajectory empty; - pub_trajectories_[robot_id].publish(empty); + pub_trajectory_command_.publish(empty); // Marks the current goal as aborted. - active_goal_map_[robot_id].setAborted(); - has_active_goal_map_[robot_id] = false; -} - -bool JointTrajectoryAction::withinGoalConstraints( - const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, - const trajectory_msgs::JointTrajectory & traj) -{ - bool rtn = false; - if (traj.points.empty()) - { - ROS_WARN("Empty joint trajectory passed to check goal constraints, return false"); - rtn = false; - } - else + active_goal_.setCanceled(); + has_active_goal_ = false; + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) { - int last_point = traj.points.size() - 1; - - if (industrial_robot_client::utils::isWithinRange( - last_trajectory_state_->joint_names, - last_trajectory_state_->actual.positions, traj.joint_names, - traj.points[last_point].positions, goal_threshold_)) - { - rtn = true; - } - else - { - rtn = false; - } + has_active_goals_map_[group_number] = false; } - return rtn; } -bool JointTrajectoryAction::withinGoalConstraints( - const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg, - const trajectory_msgs::JointTrajectory & traj, int robot_id) +bool JointTrajectoryAction::withinGoalConstraints(const trajectory_msgs::JointTrajectory & traj) { - bool rtn = false; if (traj.points.empty()) { ROS_WARN("Empty joint trajectory passed to check goal constraints, return false"); - rtn = false; + return false; } - else - { - int last_point = traj.points.size() - 1; - int group_number = robot_id; + // Assume true, and proof wrong by checking each robot group that is active in current goal + std::map groups_at_goal_state_map; + int last_point = traj.points.size() - 1; - if (industrial_robot_client::utils::isWithinRange( - robot_groups_[group_number].get_joint_names(), - last_trajectory_state_map_[group_number]->actual.positions, traj.joint_names, - traj.points[last_point].positions, goal_threshold_)) - { - rtn = true; - } - else + for (int group_number = 0; group_number < robot_groups_.size(); group_number++) + { + // Check if robot group is active in current multi-group trajectory goal. + if (has_active_goals_map_[group_number]) { - rtn = false; + ROS_DEBUG("Checking if group [%s] has reached its goal", robot_groups_[group_number].get_name().c_str()); + // Asume group is not at goal position + groups_at_goal_state_map[group_number] = false; + + // Again this assumes that joints are defined in-order within their groups. + size_t group_joints_start_idx = std::find( + traj.joint_names.begin(), + traj.joint_names.end(), + robot_groups_[group_number].get_joint_names()[0]) + - traj.joint_names.begin(); + // Get an ordered map of the robot group last feedback state. + std::map robot_group_last_state_map; + industrial_robot_client::utils::toMap(robot_groups_[group_number].get_joint_names(), + last_trajectory_state_map_[group_number]->actual.positions, + robot_group_last_state_map); + // Get an ordered map of the robot group goal state. + std::vector group_goal_positions( + traj.points[last_point].positions.begin() + group_joints_start_idx, + traj.points[last_point].positions.begin() + group_joints_start_idx + + robot_groups_[group_number].get_joint_names().size()); + std::map group_traj_last_point_map; + industrial_robot_client::utils::toMap(robot_groups_[group_number].get_joint_names(), + group_goal_positions, + group_traj_last_point_map); + // Check if group is already at goal position + if ( !industrial_robot_client::utils::isWithinRange(robot_groups_[group_number].get_joint_names(), + group_traj_last_point_map, + robot_group_last_state_map, + goal_threshold_) ) + { + // Current Robot Group is not in goal position yet + groups_at_goal_state_map[group_number] = false; + return false; // Stop checking other sub-groups + } + else + { + groups_at_goal_state_map[group_number] = true; + } } } - return rtn; + if (groups_at_goal_state_map.empty()) + { + ROS_ERROR("Multi-group goal has not a single active group"); + return false; + } + // If this point is reached, all active groups are at goal position + return true; } } // namespace joint_trajectory_action } // namespace industrial_robot_client - - diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp index 69223609..9a2dc016 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp @@ -738,4 +738,3 @@ void JointTrajectoryInterface::jointStateCB( } // namespace joint_trajectory_interface } // namespace industrial_robot_client - diff --git a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp index df6d42ce..b4ff8966 100644 --- a/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp @@ -245,7 +245,7 @@ void JointTrajectoryStreamer::streamingThread() switch (this->state_) { case TransferStates::IDLE: - ros::Duration(0.250).sleep(); // slower loop while waiting for new trajectory + ros::Duration(0.010).sleep(); // slower loop while waiting for new trajectory break; case TransferStates::STREAMING: @@ -300,4 +300,3 @@ void JointTrajectoryStreamer::trajectoryStop() } // namespace joint_trajectory_streamer } // namespace industrial_robot_client - diff --git a/motoman_driver/src/joint_streaming_node.cpp b/motoman_driver/src/joint_streaming_node.cpp index a7d3b4ee..646a1e48 100644 --- a/motoman_driver/src/joint_streaming_node.cpp +++ b/motoman_driver/src/joint_streaming_node.cpp @@ -29,23 +29,38 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include #include "motoman_driver/joint_trajectory_streamer.h" #include "industrial_utils/param_utils.h" using motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer; +std::unique_ptr motionInterface; + +void onShutdown(int signal) +{ + motionInterface->shutdown(); + ros::shutdown(); +} + int main(int argc, char** argv) { const int FS100_motion_port = 50240; // FS100 uses a "non-standard" port to comply with MotoPlus guidelines // initialize node - ros::init(argc, argv, "motion_interface"); + ros::init(argc, argv, "motion_interface", ros::init_options::NoSigintHandler); + ros::NodeHandle nh; - // launch the FS100 JointTrajectoryStreamer connection/handlers - MotomanJointTrajectoryStreamer motionInterface; + // set up custom shutdown handler + signal(SIGINT, onShutdown); + signal(SIGTERM, onShutdown); - motionInterface.init("", FS100_motion_port, false); - motionInterface.run(); + // launch the FS100 JointTrajectoryStreamer connection/handlers + motionInterface.reset(new MotomanJointTrajectoryStreamer()); + motionInterface->init("", FS100_motion_port, false); + motionInterface->run(); return 0; } diff --git a/motoman_driver/src/joint_trajectory_streamer.cpp b/motoman_driver/src/joint_trajectory_streamer.cpp index dcf8aed9..fcc5a1ea 100644 --- a/motoman_driver/src/joint_trajectory_streamer.cpp +++ b/motoman_driver/src/joint_trajectory_streamer.cpp @@ -33,6 +33,7 @@ #include "motoman_driver/simple_message/messages/motoman_motion_reply_message.h" #include "simple_message/messages/joint_traj_pt_full_message.h" #include "motoman_driver/simple_message/messages/joint_traj_pt_full_ex_message.h" +#include "motoman_msgs/MotorosError.h" #include "industrial_robot_client/utils.h" #include "industrial_utils/param_utils.h" #include @@ -93,6 +94,8 @@ bool MotomanJointTrajectoryStreamer::init(SmplMsgConnection* connection, const s srv_select_tool_ = node_.advertiseService("select_tool", &MotomanJointTrajectoryStreamer::selectToolCB, this); + motoros_error_pub_ = node_.advertise("motoros_error", 10); + return rtn; } @@ -118,6 +121,8 @@ bool MotomanJointTrajectoryStreamer::init(SmplMsgConnection* connection, const s srv_select_tool_ = node_.advertiseService("select_tool", &MotomanJointTrajectoryStreamer::selectToolCB, this); + motoros_error_pub_ = node_.advertise("motoros_error", 10); + return rtn; } @@ -414,6 +419,13 @@ bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector return true; } +void MotomanJointTrajectoryStreamer::shutdown() +{ + std_srvs::Trigger::Request req; + std_srvs::Trigger::Response res; + disableRobotCB(req, res); +} + // override send_to_robot to provide controllerReady() and setTrajMode() calls bool MotomanJointTrajectoryStreamer::send_to_robot(const std::vector& messages) { @@ -483,7 +495,7 @@ void MotomanJointTrajectoryStreamer::streamingThread() switch (this->state_) { case TransferStates::IDLE: - ros::Duration(0.250).sleep(); // slower loop while waiting for new trajectory + ros::Duration(0.010).sleep(); // slower loop while waiting for new trajectory break; case TransferStates::STREAMING: @@ -548,6 +560,13 @@ void MotomanJointTrajectoryStreamer::streamingThread() << " (#" << this->current_point_ << "): " << MotomanMotionCtrl::getErrorString(reply_status.reply_)); this->state_ = TransferStates::IDLE; + + motoman_msgs::MotorosError error; + error.code = reply_status.reply_.getResult(); + error.subcode = reply_status.reply_.getSubcode(); + error.code_description = reply_status.reply_.getResultString(); + error.subcode_description = reply_status.reply_.getSubcodeString(); + motoros_error_pub_.publish(error); break; } } @@ -640,4 +659,3 @@ bool MotomanJointTrajectoryStreamer::is_valid(const motoman_msgs::DynamicJointTr } // namespace joint_trajectory_streamer } // namespace motoman - diff --git a/motoman_msgs/CMakeLists.txt b/motoman_msgs/CMakeLists.txt index 8e7a8595..6593d869 100644 --- a/motoman_msgs/CMakeLists.txt +++ b/motoman_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ add_message_files( DynamicJointState.msg DynamicJointTrajectory.msg DynamicJointTrajectoryFeedback.msg + MotorosError.msg ) add_service_files( diff --git a/motoman_msgs/msg/MotorosError.msg b/motoman_msgs/msg/MotorosError.msg new file mode 100644 index 00000000..e5a45d6d --- /dev/null +++ b/motoman_msgs/msg/MotorosError.msg @@ -0,0 +1,6 @@ +# Message to represent errors in the motoros driver. + +int32 code +int32 subcode +string code_description +string subcode_description