Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef JOINT_TRAJECTORY_INTERFACE_H
00033 #define JOINT_TRAJECTORY_INTERFACE_H
00034
00035 #include <map>
00036 #include <vector>
00037 #include <string>
00038
00039 #include "ros/ros.h"
00040 #include "industrial_msgs/CmdJointTrajectory.h"
00041 #include "industrial_msgs/StopMotion.h"
00042 #include "simple_message/smpl_msg_connection.h"
00043 #include "simple_message/socket/tcp_client.h"
00044 #include "simple_message/messages/joint_traj_pt_message.h"
00045 #include "trajectory_msgs/JointTrajectory.h"
00046
00047 namespace industrial_robot_client
00048 {
00049 namespace joint_trajectory_interface
00050 {
00051
00052 using industrial::smpl_msg_connection::SmplMsgConnection;
00053 using industrial::tcp_client::TcpClient;
00054 using industrial::joint_traj_pt_message::JointTrajPtMessage;
00055
00062 class JointTrajectoryInterface
00063 {
00064
00065 public:
00066
00070 JointTrajectoryInterface() : default_joint_pos_(0.0), default_vel_ratio_(0.1), default_duration_(10.0) {};
00071
00077 virtual bool init();
00078
00086 virtual bool init(SmplMsgConnection* connection);
00087
00100 virtual bool init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
00101 const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());
00102
00103 virtual ~JointTrajectoryInterface();
00104
00108 virtual void run() { ros::spin(); }
00109
00110 protected:
00111
00115 virtual void trajectoryStop();
00116
00126 virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<JointTrajPtMessage>* msgs);
00127
00137 virtual bool transform(const trajectory_msgs::JointTrajectoryPoint& pt_in, trajectory_msgs::JointTrajectoryPoint* pt_out)
00138 {
00139 *pt_out = pt_in;
00140 return true;
00141 }
00142
00153 virtual bool select(const std::vector<std::string>& ros_joint_names, const trajectory_msgs::JointTrajectoryPoint& ros_pt,
00154 const std::vector<std::string>& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt);
00155
00167 virtual bool calc_speed(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity, double* rbt_duration);
00168
00178 virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_velocity);
00179
00189 virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint& pt, double* rbt_duration);
00190
00199 virtual bool send_to_robot(const std::vector<JointTrajPtMessage>& messages)=0;
00200
00207 virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg);
00208
00217 virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req,
00218 industrial_msgs::StopMotion::Response &res);
00219
00220 TcpClient default_tcp_connection_;
00221
00222 ros::NodeHandle node_;
00223 SmplMsgConnection* connection_;
00224 ros::Subscriber sub_joint_trajectory_;
00225 ros::ServiceServer srv_joint_trajectory_;
00226 ros::ServiceServer srv_stop_motion_;
00227 std::vector<std::string> all_joint_names_;
00228 double default_joint_pos_;
00229 double default_vel_ratio_;
00230 double default_duration_;
00231 std::map<std::string, double> joint_vel_limits_;
00232
00233 private:
00234 static JointTrajPtMessage create_message(int seq, std::vector<double> joint_pos, double velocity, double duration);
00235
00244 bool jointTrajectoryCB(industrial_msgs::CmdJointTrajectory::Request &req,
00245 industrial_msgs::CmdJointTrajectory::Response &res);
00246 };
00247
00248 }
00249 }
00250
00251 #endif