From 27b6f61c50c50b3e91d074f70ad33b8ac4c8136f Mon Sep 17 00:00:00 2001 From: Simon Schmeisser Date: Fri, 28 Oct 2016 19:10:58 +0200 Subject: [PATCH] poc for a feedback way from JointTrajectoryRelay to JointTrajectoryAction See #131 and #154 for discussion --- .../joint_trajectory_action.h | 14 +++++++++++++ .../joint_trajectory_streamer.h | 3 ++- .../src/joint_trajectory_action.cpp | 13 ++++++++++++ .../src/joint_trajectory_streamer.cpp | 20 +++++++++++++------ 4 files changed, 43 insertions(+), 7 deletions(-) diff --git a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h index 6eaf6b5d..1fe0562e 100644 --- a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h +++ b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_action.h @@ -97,6 +97,12 @@ class JointTrajectoryAction */ ros::Subscriber sub_robot_status_; + /** + * \brief Subscribes to the relay status (typically published by the + * robot driver) + */ + ros::Subscriber sub_relay_status_; + /** * \brief Watchdog time used to fail the action request if the robot * driver is not responding. @@ -213,6 +219,14 @@ class JointTrajectoryAction */ void robotStatusCB(const industrial_msgs::RobotStatusConstPtr &msg); + /** + * \brief Relay status callback (executed when relay status + * message received) + * + * \param msg relay status message + */ + void relayStatusCB(const industrial_msgs::RobotStatusConstPtr &msg); + /** * \brief Aborts the current action goal and sends a stop command * (empty message) to the robot driver. diff --git a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h index 87b5fcda..48105ad6 100644 --- a/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h +++ b/industrial_robot_client/include/industrial_robot_client/joint_trajectory_streamer.h @@ -74,7 +74,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface /** * \brief Default constructor * - * \param min_buffer_size minimum number of points as required by robot implementation + * \param min_buffer_size minimum number of points as required by robot implementation */ JointTrajectoryStreamer(int min_buffer_size = 1) : min_buffer_size_(min_buffer_size) {}; @@ -114,6 +114,7 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface TransferState state_; ros::Time streaming_start_; int min_buffer_size_; + ros::Publisher feedback_publisher_; }; } //joint_trajectory_streamer diff --git a/industrial_robot_client/src/joint_trajectory_action.cpp b/industrial_robot_client/src/joint_trajectory_action.cpp index 0f92b958..78e9c5e4 100644 --- a/industrial_robot_client/src/joint_trajectory_action.cpp +++ b/industrial_robot_client/src/joint_trajectory_action.cpp @@ -62,6 +62,7 @@ JointTrajectoryAction::JointTrajectoryAction() : pub_trajectory_command_ = node_.advertise("joint_path_command", 1); sub_trajectory_state_ = node_.subscribe("feedback_states", 1, &JointTrajectoryAction::controllerStateCB, this); sub_robot_status_ = node_.subscribe("robot_status", 1, &JointTrajectoryAction::robotStatusCB, this); + sub_relay_status_ = node_.subscribe("relay_feedback", 1, &JointTrajectoryAction::relayStatusCB, this); watchdog_timer_ = node_.createTimer(ros::Duration(WATCHDOG_PERIOD_), &JointTrajectoryAction::watchdog, this, true); action_server_.start(); @@ -77,6 +78,18 @@ void JointTrajectoryAction::robotStatusCB(const industrial_msgs::RobotStatusCons has_moved_once_ = has_moved_once_ ? true : (last_robot_status_->in_motion.val == industrial_msgs::TriState::TRUE); } +void JointTrajectoryAction::relayStatusCB(const industrial_msgs::RobotStatusConstPtr &msg) +{ + ROS_ERROR("FEEDBACK from relay received"); + if (msg->in_error.val == industrial_msgs::TriState::TRUE) { + ROS_ERROR("ERROR in relay"); + control_msgs::FollowJointTrajectoryResult rslt; + rslt.error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED; + active_goal_.setAborted(rslt, "Error in trajectory relay"); + has_active_goal_ = false; + } +} + void JointTrajectoryAction::watchdog(const ros::TimerEvent &e) { // Some debug logging diff --git a/industrial_robot_client/src/joint_trajectory_streamer.cpp b/industrial_robot_client/src/joint_trajectory_streamer.cpp index abf7fe1c..85b57fb4 100644 --- a/industrial_robot_client/src/joint_trajectory_streamer.cpp +++ b/industrial_robot_client/src/joint_trajectory_streamer.cpp @@ -30,6 +30,7 @@ */ #include "industrial_robot_client/joint_trajectory_streamer.h" +#include "industrial_msgs/RobotStatus.h" using industrial::simple_message::SimpleMessage; @@ -47,6 +48,8 @@ bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vec rtn &= JointTrajectoryInterface::init(connection, joint_names, velocity_limits); + feedback_publisher_ = node_.advertise("relay_feedback", 1); + this->mutex_.lock(); this->current_point_ = 0; this->state_ = TransferStates::IDLE; @@ -73,14 +76,19 @@ void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTraj ROS_DEBUG("Current state is: %d", state); if (TransferStates::IDLE != state) { - if (msg->points.empty()) + if (msg->points.empty()) { ROS_INFO("Empty trajectory received, canceling current trajectory"); - else - ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion."); - this->mutex_.lock(); - trajectoryStop(); - this->mutex_.unlock(); + this->mutex_.lock(); + trajectoryStop(); + this->mutex_.unlock(); + } else { + ROS_ERROR("Trajectory splicing not yet implemented, finishing current motion. Try again later"); + industrial_msgs::RobotStatus feedback; + feedback.in_error.val = industrial_msgs::TriState::TRUE; + feedback_publisher_.publish(feedback); + } + return; }