joint_trajectory_interface.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Southwest Research Institute
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions are met:
9  *
10  * * Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * * Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in the
14  * documentation and/or other materials provided with the distribution.
15  * * Neither the name of the Southwest Research Institute, nor the names
16  * of its contributors may be used to endorse or promote products derived
17  * from this software without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29  * POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #ifndef JOINT_TRAJECTORY_INTERFACE_H
33 #define JOINT_TRAJECTORY_INTERFACE_H
34 
35 #include <map>
36 #include <vector>
37 #include <string>
38 
39 #include "ros/ros.h"
40 #include "industrial_msgs/CmdJointTrajectory.h"
41 #include "industrial_msgs/StopMotion.h"
42 #include "sensor_msgs/JointState.h"
46 #include "trajectory_msgs/JointTrajectory.h"
47 
49 {
50 namespace joint_trajectory_interface
51 {
52 
57 
65 {
66 
67 public:
68 
73 
84  virtual bool init(std::string default_ip = "", int default_port = StandardSocketPorts::MOTION);
85 
86 
94  virtual bool init(SmplMsgConnection* connection);
95 
108  virtual bool init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
109  const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());
110 
111  virtual ~JointTrajectoryInterface();
112 
116  virtual void run() { ros::spin(); }
117 
118 protected:
119 
123  virtual void trajectoryStop();
124 
134  virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<JointTrajPtMessage>* msgs);
135 
145  virtual bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out)
146  {
147  *pt_out = pt_in; // by default, no transform is applied
148  return true;
149  }
150 
161  virtual bool select(const std::vector<std::string>& ros_joint_names, const trajectory_msgs::JointTrajectoryPoint& ros_pt,
162  const std::vector<std::string>& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt);
163 
175  virtual bool calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration);
176 
186  virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity);
187 
197  virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration);
198 
207  virtual bool send_to_robot(const std::vector<JointTrajPtMessage>& messages)=0;
208 
215  virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
216 
225  virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req,
226  industrial_msgs::StopMotion::Response &res);
227 
234  virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj);
235 
236  /*
237  * \brief Callback for JointState topic
238  *
239  * \param msg JointState message
240  */
241  virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg);
242 
244 
247  ros::Subscriber sub_cur_pos_; // handle for joint-state topic subscription
248  ros::Subscriber sub_joint_trajectory_; // handle for joint-trajectory topic subscription
249  ros::ServiceServer srv_joint_trajectory_; // handle for joint-trajectory service
250  ros::ServiceServer srv_stop_motion_; // handle for stop_motion service
251  std::vector<std::string> all_joint_names_;
252  double default_joint_pos_; // default position to use for "dummy joints", if none specified
253  double default_vel_ratio_; // default velocity ratio to use for joint commands, if no velocity or max_vel specified
254  double default_duration_; // default duration to use for joint commands, if no
255  std::map<std::string, double> joint_vel_limits_; // cache of max joint velocities from URDF
256  sensor_msgs::JointState cur_joint_pos_; // cache of last received joint state
257 
258 
259 private:
260  static JointTrajPtMessage create_message(int seq, std::vector<double> joint_pos, double velocity, double duration);
261 
270  bool jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
271  industrial_msgs::CmdJointTrajectory::Response &res);
272 };
273 
274 } //joint_trajectory_interface
275 } //industrial_robot_client
276 
277 #endif /* JOINT_TRAJECTORY_INTERFACE_H */
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::cur_joint_pos_
sensor_msgs::JointState cur_joint_pos_
Definition: joint_trajectory_interface.h:256
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::jointTrajectoryCB
virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Callback function registered to ROS topic-subscribe. Transform message into SimpleMessage objects and...
Definition: joint_trajectory_interface.cpp:131
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::sub_joint_trajectory_
ros::Subscriber sub_joint_trajectory_
Definition: joint_trajectory_interface.h:248
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::calc_duration
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,...
Definition: joint_trajectory_interface.cpp:282
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::select
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.
Definition: joint_trajectory_interface.cpp:184
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::connection_
SmplMsgConnection * connection_
Definition: joint_trajectory_interface.h:246
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_joint_pos_
double default_joint_pos_
Definition: joint_trajectory_interface.h:252
ros.h
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::stopMotionCB
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.
Definition: joint_trajectory_interface.cpp:327
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::jointStateCB
virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
Definition: joint_trajectory_interface.cpp:367
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::sub_cur_pos_
ros::Subscriber sub_cur_pos_
Definition: joint_trajectory_interface.h:247
smpl_msg_connection.h
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::JointTrajectoryInterface
JointTrajectoryInterface()
Default constructor.
Definition: joint_trajectory_interface.h:72
ros::ServiceServer
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::~JointTrajectoryInterface
virtual ~JointTrajectoryInterface()
Definition: joint_trajectory_interface.cpp:112
joint_traj_pt_message.h
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_tcp_connection_
TcpClient default_tcp_connection_
Definition: joint_trajectory_interface.h:243
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::trajectory_to_msgs
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....
Definition: joint_trajectory_interface.cpp:152
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::run
virtual void run()
Begin processing messages and publishing topics.
Definition: joint_trajectory_interface.h:116
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_duration_
double default_duration_
Definition: joint_trajectory_interface.h:254
industrial::simple_socket::StandardSocketPorts
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::create_message
static JointTrajPtMessage create_message(int seq, std::vector< double > joint_pos, double velocity, double duration)
Definition: joint_trajectory_interface.cpp:298
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::calc_speed
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 r...
Definition: joint_trajectory_interface.cpp:224
tcp_client.h
industrial::smpl_msg_connection::SmplMsgConnection
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::calc_velocity
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 r...
Definition: joint_trajectory_interface.cpp:235
industrial_robot_client
Definition: joint_relay_handler.h:46
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::transform
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 join...
Definition: joint_trajectory_interface.h:145
industrial::joint_traj_pt_message::JointTrajPtMessage
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::all_joint_names_
std::vector< std::string > all_joint_names_
Definition: joint_trajectory_interface.h:251
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::send_to_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 ...
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface
Message handler that relays joint trajectories to the robot controller.
Definition: joint_trajectory_interface.h:64
ros::spin
ROSCPP_DECL void spin()
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::node_
ros::NodeHandle node_
Definition: joint_trajectory_interface.h:245
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::joint_vel_limits_
std::map< std::string, double > joint_vel_limits_
Definition: joint_trajectory_interface.h:255
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::srv_stop_motion_
ros::ServiceServer srv_stop_motion_
Definition: joint_trajectory_interface.h:250
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::is_valid
virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj)
Validate that trajectory command meets minimum requirements.
Definition: joint_trajectory_interface.cpp:338
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::srv_joint_trajectory_
ros::ServiceServer srv_joint_trajectory_
Definition: joint_trajectory_interface.h:249
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::init
virtual bool init(std::string default_ip="", int default_port=StandardSocketPorts::MOTION)
Initialize robot connection using default method.
Definition: joint_trajectory_interface.cpp:51
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::trajectoryStop
virtual void trajectoryStop()
Send a stop command to the robot.
Definition: joint_trajectory_interface.cpp:315
industrial::tcp_client::TcpClient
ros::NodeHandle
ros::Subscriber
industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface::default_vel_ratio_
double default_vel_ratio_
Definition: joint_trajectory_interface.h:253


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Wed Mar 2 2022 00:24:59