57 namespace joint_trajectory_streamer
62 const double pos_stale_time_ = 1.0;
63 const double start_pos_tol_ = 1e-4;
66 #define ROS_ERROR_RETURN(rtn, ...) do {ROS_ERROR(__VA_ARGS__); return(rtn);} while (0) // NOLINT(whitespace/braces) 70 const std::map<std::string, double> &velocity_limits)
74 ROS_INFO(
"MotomanJointTrajectoryStreamer: init");
76 this->robot_groups_ = robot_groups;
80 for (
size_t i = 0; i < robot_groups_.size(); i++)
84 int robot_id = robot_groups_[i].get_group_id();
85 rtn &= motion_ctrl.
init(connection, robot_id);
99 const std::map<std::string, double> &velocity_limits)
103 ROS_INFO(
"MotomanJointTrajectoryStreamer: init");
127 std_srvs::Trigger::Response &res)
136 res.message =
"Motoman robot was NOT disabled. Please re-examine and retry.";
141 res.message =
"Motoman robot is now disabled and will NOT accept motion commands.";
155 res.message =
"Motoman robot was NOT enabled. Please re-examine and retry.";
160 res.message =
"Motoman robot is now enabled and will accept motion commands.";
175 if (!pt.positions.empty())
180 ROS_ERROR_RETURN(
false,
"Failed to copy position data to JointTrajPtFullMessage");
186 if (!pt.velocities.empty())
191 ROS_ERROR_RETURN(
false,
"Failed to copy velocity data to JointTrajPtFullMessage");
197 if (!pt.accelerations.empty())
202 ROS_ERROR_RETURN(
false,
"Failed to copy acceleration data to JointTrajPtFullMessage");
210 msg_data.
setTime(pt.time_from_start.toSec());
215 jtpf_msg.
init(msg_data);
225 std::vector<industrial::joint_traj_pt_full::JointTrajPtFull> msg_data_vector;
229 int num_groups = point.num_groups;
231 for (
int i = 0; i < num_groups; i++)
235 motoman_msgs::DynamicJointsGroup pt;
237 motoman_msgs::DynamicJointPoint dpoint;
239 pt = point.groups[i];
241 if (pt.positions.size() < 10)
243 int size_to_complete = 10 - pt.positions.size();
245 std::vector<double> positions(size_to_complete, 0.0);
246 std::vector<double> velocities(size_to_complete, 0.0);
247 std::vector<double> accelerations(size_to_complete, 0.0);
249 pt.positions.insert(pt.positions.end(), positions.begin(), positions.end());
250 pt.velocities.insert(pt.velocities.end(), velocities.begin(), velocities.end());
251 pt.accelerations.insert(pt.accelerations.end(), accelerations.begin(), accelerations.end());
255 if (!pt.positions.empty())
260 ROS_ERROR_RETURN(
false,
"Failed to copy position data to JointTrajPtFullMessage");
265 if (!pt.velocities.empty())
270 ROS_ERROR_RETURN(
false,
"Failed to copy velocity data to JointTrajPtFullMessage");
276 if (!pt.accelerations.empty())
281 ROS_ERROR_RETURN(
false,
"Failed to copy acceleration data to JointTrajPtFullMessage");
289 msg_data.
setTime(pt.time_from_start.toSec());
292 msg_data_vector.push_back(msg_data);
298 jtpf_msg_ex.
init(msg_data_ex);
309 if (!pt.positions.empty())
314 ROS_ERROR_RETURN(
false,
"Failed to copy position data to JointTrajPtFullMessage");
320 if (!pt.velocities.empty())
325 ROS_ERROR_RETURN(
false,
"Failed to copy velocity data to JointTrajPtFullMessage");
331 if (!pt.accelerations.empty())
336 ROS_ERROR_RETURN(
false,
"Failed to copy acceleration data to JointTrajPtFullMessage");
345 msg_data.
setTime(pt.time_from_start.toSec());
349 jtpf_msg.
init(msg_data);
358 ROS_ERROR_RETURN(
false,
"Failed to copy to JointData. Len (%d) out of range (0 to %d)",
362 for (
size_t i = 0; i < vec.size(); ++i)
373 ROS_ERROR_RETURN(
false,
"Failed to initialize MotoRos motion, trajectory execution ABORTED. If safe, call the " 374 "'robot_enable' service to (re-)enable Motoplus motion and retry.");
382 int connectRetryCount = 1;
384 ROS_INFO(
"Starting Motoman joint trajectory streamer thread");
390 if (connectRetryCount-- > 0)
392 ROS_INFO(
"Connecting to robot motion server");
397 connectRetryCount = 0;
398 else if (connectRetryCount <= 0)
400 ROS_ERROR(
"Timeout connecting to robot controller. Send new motion command to retry.");
401 this->
state_ = TransferStates::IDLE;
412 case TransferStates::IDLE:
416 case TransferStates::STREAMING:
419 ROS_INFO(
"Trajectory streaming complete, setting state to IDLE");
420 this->
state_ = TransferStates::IDLE;
426 ROS_DEBUG(
"Robot disconnected. Attempting reconnect...");
427 connectRetryCount = 5;
436 ROS_WARN(
"Failed sent joint point, will try again");
440 if (!reply_status.
init(reply))
442 ROS_ERROR(
"Aborting trajectory: Unable to parse JointTrajectoryPoint reply");
443 this->
state_ = TransferStates::IDLE;
449 ROS_DEBUG(
"Point[%d of %d] sent to controller",
460 this->
state_ = TransferStates::IDLE;
466 ROS_ERROR(
"Joint trajectory streamer: unknown state");
467 this->
state_ = TransferStates::IDLE;
472 ROS_WARN(
"Exiting trajectory streamer thread");
478 this->
state_ = TransferStates::IDLE;
485 if (!JointTrajectoryInterface::is_valid(traj))
488 for (
size_t i = 0; i < traj.points.size(); ++i)
490 const trajectory_msgs::JointTrajectoryPoint &pt = traj.points[i];
493 if (pt.velocities.empty())
494 ROS_ERROR_RETURN(
false,
"Validation failed: Missing velocity data for trajectory pt %lu", i);
498 ROS_ERROR_RETURN(
false,
"Validation failed: Can't get current robot position.");
503 traj.joint_names, traj.points[0].positions,
506 ROS_ERROR_RETURN(
false,
"Validation failed: Trajectory doesn't start at current position.");
513 if (!JointTrajectoryInterface::is_valid(traj))
517 for (
size_t i = 0; i < traj.points.size(); ++i)
519 for (
int gr = 0; gr < traj.points[i].num_groups; gr++)
521 const motoman_msgs::DynamicJointsGroup &pt = traj.points[i].groups[gr];
522 time_stamp = cur_joint_pos_map_[pt.group_number].header.stamp;
524 group_number = pt.group_number;
526 if (pt.velocities.empty())
527 ROS_ERROR_RETURN(
false,
"Validation failed: Missing velocity data for trajectory pt %lu", i);
532 if (!IRC_utils::isWithinRange(cur_joint_pos_map_[group_number].name, cur_joint_pos_map_[group_number].position,
533 traj.joint_names, traj.points[0].groups[gr].positions,
536 ROS_ERROR_RETURN(
false,
"Validation failed: Trajectory doesn't start at current position.");
542 ROS_ERROR_RETURN(
false,
"Validation failed: Can't get current robot position.");
std::map< int, MotomanMotionCtrl > motion_ctrl_map_
Class encapsulated joint trajectory point data. The point data serves as a waypoint along a trajector...
void setSequence(industrial::shared_types::shared_int sequence)
std::vector< double > values
virtual bool send_to_robot(const std::vector< SimpleMessage > &messages)
Send trajectory to robot, using this node's robot-connection. Specific method must be implemented in ...
~MotomanJointTrajectoryStreamer()
SmplMsgConnection * connection_
ros::ServiceServer disabler_
Service used to disable the robot controller. When disabled, all incoming goals are ignored...
void setTime(industrial::shared_types::shared_real time)
virtual bool create_message(int seq, const trajectory_msgs::JointTrajectoryPoint &pt, SimpleMessage *msg)
Create SimpleMessage for sending to the robot.
void clearAccelerations()
sensor_msgs::JointState cur_joint_pos_
Wrapper class around Motoman-specific motion control commands.
void setVelocities(industrial::joint_data::JointData &velocities)
void setSequence(industrial::shared_types::shared_int sequence)
Sets joint trajectory point sequence number.
bool init(industrial::simple_message::SimpleMessage &msg)
Initializes message from a simple message.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
virtual bool toRequest(industrial::simple_message::SimpleMessage &msg)
Enumeration of motion reply result codes.
static bool VectorToJointData(const std::vector< double > &vec, industrial::joint_data::JointData &joints)
bool disableRobotCB(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Disable the robot. Response is true if the state was flipped or false if the state has not changed...
bool sendAndReceiveMsg(industrial::simple_message::SimpleMessage &send, industrial::simple_message::SimpleMessage &recv, bool verbose=false)
bool is_valid(const trajectory_msgs::JointTrajectory &traj)
#define ROS_ERROR_RETURN(rtn,...)
void setNumGroups(industrial::shared_types::shared_int num_groups)
Sets num_groups Number of groups attached to the controller.
motoman::simple_message::motion_reply::MotionReply reply_
bool init(SmplMsgConnection *connection, int robot_id)
bool setTrajMode(bool enable)
bool init(industrial::simple_message::SimpleMessage &msg)
bool send_to_robot(const std::vector< JointTrajPtMessage > &messages)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
bool setJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real value)
industrial::shared_types::shared_int getResult() const
Returns motion-control result code.
bool enableRobotCB(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
Enable the robot. Response is true if the state was flipped or false if the state has not changed...
static std::string getErrorString(const MotionReply &reply)
virtual bool create_message_ex(int seq, const motoman_msgs::DynamicJointPoint &point, SimpleMessage *msg)
#define ROS_WARN_STREAM(args)
Class encapsulated joint trajectory extended point message generation methods (either to or from a in...
virtual bool init(SmplMsgConnection *connection, const std::vector< std::string > &joint_names, const std::map< std::string, double > &velocity_limits=std::map< std::string, double >())
void setRobotID(industrial::shared_types::shared_int robot_id)
void setPositions(industrial::joint_data::JointData &positions)
bool init(int msgType, int commType, int replyCode, industrial::byte_array::ByteArray &data)
virtual bool isConnected()=0
virtual void streamingThread()
std::vector< JointTrajPtMessage > current_traj_
int getMaxNumJoints() const
void setMultiJointTrajPtData(std::vector< industrial::joint_traj_pt_full::JointTrajPtFull > joint_trajectory_points)
Class encapsulated motoman motion reply message generation methods (either to or from a industrial::s...
industrial::byte_array::ByteArray & getData()
MotomanMotionCtrl motion_ctrl_
#define ROS_ERROR_STREAM(args)
ros::ServiceServer enabler_
Service used to enable the robot controller. When disabled, all incoming goals are ignored...
bool init(industrial::simple_message::SimpleMessage &msg)
Initializes message from a simple message.
virtual bool init(SmplMsgConnection *connection, const std::vector< std::string > &joint_names, const std::map< std::string, double > &velocity_limits=std::map< std::string, double >())
Class initializer.
void setAccelerations(industrial::joint_data::JointData &accelerations)
virtual bool makeConnect()=0