Message handler that relays joint trajectories to the robot controller. More...
#include <joint_trajectory_interface.h>
Public Member Functions | |
virtual bool | init () |
Initialize robot connection using default method. | |
virtual bool | init (SmplMsgConnection *connection) |
Initialize robot connection using specified method. | |
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. | |
JointTrajectoryInterface () | |
Default constructor. | |
virtual void | run () |
Begin processing messages and publishing topics. | |
virtual | ~JointTrajectoryInterface () |
Protected Member Functions | |
virtual bool | calc_duration (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_duration) |
Compute the expected move duration for communication to the robot. If unneeded by the robot server, set to 0 (or any value). | |
virtual bool | calc_speed (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_velocity, double *rbt_duration) |
Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. For flexibility, the robot command message contains both "velocity" and "duration" fields. The specific robot implementation can utilize either or both of these fields, as appropriate. | |
virtual bool | calc_velocity (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_velocity) |
Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. If unneeded by the robot server, set to 0 (or any value). | |
virtual void | jointTrajectoryCB (const trajectory_msgs::JointTrajectoryConstPtr &msg) |
Callback function registered to ROS topic-subscribe. Transform message into SimpleMessage objects and send commands to robot. | |
virtual bool | select (const std::vector< std::string > &ros_joint_names, const trajectory_msgs::JointTrajectoryPoint &ros_pt, const std::vector< std::string > &rbt_joint_names, trajectory_msgs::JointTrajectoryPoint *rbt_pt) |
Select specific joints for sending to the robot. | |
virtual bool | send_to_robot (const std::vector< JointTrajPtMessage > &messages)=0 |
Send trajectory to robot, using this node's robot-connection. Specific method must be implemented in a derived class (e.g. streaming, download, etc.) | |
virtual bool | stopMotionCB (industrial_msgs::StopMotion::Request &req, industrial_msgs::StopMotion::Response &res) |
Callback function registered to ROS stopMotion service Sends stop-motion command to robot. | |
virtual bool | trajectory_to_msgs (const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs) |
Convert ROS trajectory message into stream of JointTrajPtMessages for sending to robot. Also includes various joint transforms that can be overridden for robot-specific behavior. | |
virtual void | trajectoryStop () |
Send a stop command to the robot. | |
virtual bool | transform (const trajectory_msgs::JointTrajectoryPoint &pt_in, trajectory_msgs::JointTrajectoryPoint *pt_out) |
Transform joint positions before publishing. Can be overridden to implement, e.g. robot-specific joint coupling. | |
Protected Attributes | |
std::vector< std::string > | all_joint_names_ |
SmplMsgConnection * | connection_ |
double | default_duration_ |
double | default_joint_pos_ |
TcpClient | default_tcp_connection_ |
double | default_vel_ratio_ |
std::map< std::string, double > | joint_vel_limits_ |
ros::NodeHandle | node_ |
ros::ServiceServer | srv_joint_trajectory_ |
ros::ServiceServer | srv_stop_motion_ |
ros::Subscriber | sub_joint_trajectory_ |
Private Member Functions | |
bool | jointTrajectoryCB (industrial_msgs::CmdJointTrajectory::Request &req, industrial_msgs::CmdJointTrajectory::Response &res) |
Callback function registered to ROS CmdJointTrajectory service Duplicates message-topic functionality, but in service form. | |
Static Private Member Functions | |
static JointTrajPtMessage | create_message (int seq, std::vector< double > joint_pos, double velocity, double duration) |
Message handler that relays joint trajectories to the robot controller.
THIS CLASS IS NOT THREAD-SAFE
Definition at line 62 of file joint_trajectory_interface.h.
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::JointTrajectoryInterface | ( | ) | [inline] |
Default constructor.
Definition at line 70 of file joint_trajectory_interface.h.
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::~JointTrajectoryInterface | ( | ) | [virtual] |
Definition at line 92 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::calc_duration | ( | const trajectory_msgs::JointTrajectoryPoint & | pt, |
double * | rbt_duration | ||
) | [protected, virtual] |
Compute the expected move duration for communication to the robot. If unneeded by the robot server, set to 0 (or any value).
[in] | pt | trajectory point data, in order/count expected by robot connection |
[out] | rbt_duration | computed move duration for robot message (if needed by robot) |
Definition at line 267 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::calc_speed | ( | const trajectory_msgs::JointTrajectoryPoint & | pt, |
double * | rbt_velocity, | ||
double * | rbt_duration | ||
) | [protected, virtual] |
Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. For flexibility, the robot command message contains both "velocity" and "duration" fields. The specific robot implementation can utilize either or both of these fields, as appropriate.
[in] | pt | trajectory point data, in order/count expected by robot connection |
[out] | rbt_velocity | computed velocity scalar for robot message (if needed by robot) |
[out] | rbt_duration | computed move duration for robot message (if needed by robot) |
Definition at line 200 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::calc_velocity | ( | const trajectory_msgs::JointTrajectoryPoint & | pt, |
double * | rbt_velocity | ||
) | [protected, virtual] |
Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. If unneeded by the robot server, set to 0 (or any value).
[in] | pt | trajectory point data, in order/count expected by robot connection |
[out] | rbt_velocity | computed velocity scalar for robot message (if needed by robot) |
Definition at line 211 of file joint_trajectory_interface.cpp.
JointTrajPtMessage industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::create_message | ( | int | seq, |
std::vector< double > | joint_pos, | ||
double | velocity, | ||
double | duration | ||
) | [static, private] |
Definition at line 283 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::init | ( | ) | [virtual] |
Initialize robot connection using default method.
Definition at line 49 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::init | ( | SmplMsgConnection * | connection | ) | [virtual] |
Initialize robot connection using specified method.
connection | new robot-connection instance (ALREADY INITIALIZED). |
Definition at line 68 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::init | ( | SmplMsgConnection * | connection, |
const std::vector< std::string > & | joint_names, | ||
const std::map< std::string, double > & | velocity_limits = std::map<std::string, double>() |
||
) | [virtual] |
Class initializer.
connection | simple message connection that will be used to send commands to robot (ALREADY INITIALIZED) |
joint_names | list of expected joint-names.
|
velocity_limits | map of maximum velocities for each joint
|
Reimplemented in industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.
Definition at line 77 of file joint_trajectory_interface.cpp.
void industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::jointTrajectoryCB | ( | const trajectory_msgs::JointTrajectoryConstPtr & | msg | ) | [protected, virtual] |
Callback function registered to ROS topic-subscribe. Transform message into SimpleMessage objects and send commands to robot.
msg | JointTrajectory message from ROS trajectory-planner |
Reimplemented in industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.
Definition at line 111 of file joint_trajectory_interface.cpp.
Callback function registered to ROS CmdJointTrajectory service Duplicates message-topic functionality, but in service form.
req | CmdJointTrajectory request from service call |
res | CmdJointTrajectory response to service call |
Definition at line 98 of file joint_trajectory_interface.cpp.
virtual void industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::run | ( | void | ) | [inline, virtual] |
Begin processing messages and publishing topics.
Definition at line 108 of file joint_trajectory_interface.h.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::select | ( | const std::vector< std::string > & | ros_joint_names, |
const trajectory_msgs::JointTrajectoryPoint & | ros_pt, | ||
const std::vector< std::string > & | rbt_joint_names, | ||
trajectory_msgs::JointTrajectoryPoint * | rbt_pt | ||
) | [protected, virtual] |
Select specific joints for sending to the robot.
[in] | ros_joint_names | joint names from ROS command |
[in] | ros_pt | target pos/vel from ROS command |
[in] | rbt_joint_names | joint names, in order/count expected by robot connection |
[out] | rbt_pt | target pos/vel, matching rbt_joint_names |
Definition at line 160 of file joint_trajectory_interface.cpp.
virtual bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::send_to_robot | ( | const std::vector< JointTrajPtMessage > & | messages | ) | [protected, pure virtual] |
Send trajectory to robot, using this node's robot-connection. Specific method must be implemented in a derived class (e.g. streaming, download, etc.)
messages | List of SimpleMessage JointTrajPtMessages to send to robot. |
Implemented in industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer, and industrial_robot_client::joint_trajectory_downloader::JointTrajectoryDownloader.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::stopMotionCB | ( | industrial_msgs::StopMotion::Request & | req, |
industrial_msgs::StopMotion::Response & | res | ||
) | [protected, virtual] |
Callback function registered to ROS stopMotion service Sends stop-motion command to robot.
req | StopMotion request from service call |
res | StopMotion response to service call |
Definition at line 312 of file joint_trajectory_interface.cpp.
bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::trajectory_to_msgs | ( | const trajectory_msgs::JointTrajectoryConstPtr & | traj, |
std::vector< JointTrajPtMessage > * | msgs | ||
) | [protected, virtual] |
Convert ROS trajectory message into stream of JointTrajPtMessages for sending to robot. Also includes various joint transforms that can be overridden for robot-specific behavior.
[in] | traj | ROS JointTrajectory message |
[out] | msgs | list of JointTrajPtMessages for sending to robot |
Reimplemented in industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.
Definition at line 132 of file joint_trajectory_interface.cpp.
void industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::trajectoryStop | ( | ) | [protected, virtual] |
Send a stop command to the robot.
Reimplemented in industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.
Definition at line 300 of file joint_trajectory_interface.cpp.
virtual bool industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::transform | ( | const trajectory_msgs::JointTrajectoryPoint & | pt_in, |
trajectory_msgs::JointTrajectoryPoint * | pt_out | ||
) | [inline, protected, virtual] |
Transform joint positions before publishing. Can be overridden to implement, e.g. robot-specific joint coupling.
[in] | pt_in | trajectory-point, in same order as expected for robot-connection. |
[out] | pt_out | transformed trajectory-point (in same order/count as input positions) |
Definition at line 137 of file joint_trajectory_interface.h.
std::vector<std::string> industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::all_joint_names_ [protected] |
Definition at line 227 of file joint_trajectory_interface.h.
SmplMsgConnection* industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::connection_ [protected] |
Definition at line 223 of file joint_trajectory_interface.h.
double industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_duration_ [protected] |
Definition at line 230 of file joint_trajectory_interface.h.
double industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_joint_pos_ [protected] |
Definition at line 228 of file joint_trajectory_interface.h.
TcpClient industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_tcp_connection_ [protected] |
Definition at line 220 of file joint_trajectory_interface.h.
double industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_vel_ratio_ [protected] |
Definition at line 229 of file joint_trajectory_interface.h.
std::map<std::string, double> industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::joint_vel_limits_ [protected] |
Definition at line 231 of file joint_trajectory_interface.h.
ros::NodeHandle industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::node_ [protected] |
Definition at line 222 of file joint_trajectory_interface.h.
ros::ServiceServer industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::srv_joint_trajectory_ [protected] |
Definition at line 225 of file joint_trajectory_interface.h.
ros::ServiceServer industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::srv_stop_motion_ [protected] |
Definition at line 226 of file joint_trajectory_interface.h.
ros::Subscriber industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::sub_joint_trajectory_ [protected] |
Definition at line 224 of file joint_trajectory_interface.h.