Message handler that relays joint trajectories to the robot controller. More...
#include <joint_trajectory_interface.h>
Public Member Functions | |
virtual bool | init (SmplMsgConnection *connection) |
Initialize robot connection using specified method. More... | |
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. More... | |
virtual bool | init (std::string default_ip="", int default_port=StandardSocketPorts::MOTION) |
Initialize robot connection using default method. More... | |
JointTrajectoryInterface () | |
Default constructor. More... | |
virtual void | run () |
Begin processing messages and publishing topics. More... | |
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). More... | |
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. More... | |
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). More... | |
virtual bool | is_valid (const trajectory_msgs::JointTrajectory &traj) |
Validate that trajectory command meets minimum requirements. More... | |
virtual void | jointStateCB (const sensor_msgs::JointStateConstPtr &msg) |
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. More... | |
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. More... | |
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.) More... | |
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. More... | |
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. More... | |
virtual void | trajectoryStop () |
Send a stop command to the robot. More... | |
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. More... | |
Protected Attributes | |
std::vector< std::string > | all_joint_names_ |
SmplMsgConnection * | connection_ |
sensor_msgs::JointState | cur_joint_pos_ |
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_cur_pos_ |
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. More... | |
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 64 of file joint_trajectory_interface.h.
|
inline |
Default constructor.
Definition at line 72 of file joint_trajectory_interface.h.
|
virtual |
Definition at line 112 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
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 282 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
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 224 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
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 235 of file joint_trajectory_interface.cpp.
|
staticprivate |
Definition at line 298 of file joint_trajectory_interface.cpp.
|
virtual |
Initialize robot connection using specified method.
connection | new robot-connection instance (ALREADY INITIALIZED). |
Definition at line 80 of file joint_trajectory_interface.cpp.
|
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 92 of file joint_trajectory_interface.cpp.
|
virtual |
Initialize robot connection using default method.
default_ip | default IP address to use for robot connection [OPTIONAL]
|
default_port | default port to use for robot connection [OPTIONAL]
|
Definition at line 51 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
Validate that trajectory command meets minimum requirements.
traj | incoming trajectory |
Definition at line 338 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
Definition at line 367 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
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 131 of file joint_trajectory_interface.cpp.
|
private |
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 118 of file joint_trajectory_interface.cpp.
|
inlinevirtual |
Begin processing messages and publishing topics.
Definition at line 116 of file joint_trajectory_interface.h.
|
protectedvirtual |
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 184 of file joint_trajectory_interface.cpp.
|
protectedpure 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.
|
protectedvirtual |
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 327 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
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 152 of file joint_trajectory_interface.cpp.
|
protectedvirtual |
Send a stop command to the robot.
Reimplemented in industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.
Definition at line 315 of file joint_trajectory_interface.cpp.
|
inlineprotectedvirtual |
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 145 of file joint_trajectory_interface.h.
|
protected |
Definition at line 251 of file joint_trajectory_interface.h.
|
protected |
Definition at line 246 of file joint_trajectory_interface.h.
|
protected |
Definition at line 256 of file joint_trajectory_interface.h.
|
protected |
Definition at line 254 of file joint_trajectory_interface.h.
|
protected |
Definition at line 252 of file joint_trajectory_interface.h.
|
protected |
Definition at line 243 of file joint_trajectory_interface.h.
|
protected |
Definition at line 253 of file joint_trajectory_interface.h.
|
protected |
Definition at line 255 of file joint_trajectory_interface.h.
|
protected |
Definition at line 245 of file joint_trajectory_interface.h.
|
protected |
Definition at line 249 of file joint_trajectory_interface.h.
|
protected |
Definition at line 250 of file joint_trajectory_interface.h.
|
protected |
Definition at line 247 of file joint_trajectory_interface.h.
|
protected |
Definition at line 248 of file joint_trajectory_interface.h.