32 #ifndef MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_INTERFACE_H 33 #define MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_INTERFACE_H 40 #include "industrial_msgs/CmdJointTrajectory.h" 41 #include "motoman_msgs/CmdJointTrajectoryEx.h" 42 #include "industrial_msgs/StopMotion.h" 43 #include "sensor_msgs/JointState.h" 48 #include "trajectory_msgs/JointTrajectory.h" 53 namespace joint_trajectory_interface
68 class JointTrajectoryInterface
75 typedef std::map<int, RobotGroup>::iterator it_type;
87 virtual bool init(std::string default_ip =
"",
int default_port = StandardSocketPorts::MOTION,
88 bool version_0 =
false);
97 virtual bool init(SmplMsgConnection* connection);
111 virtual bool init(SmplMsgConnection* connection,
const std::vector<std::string> &joint_names,
112 const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());
126 virtual bool init(SmplMsgConnection* connection,
const std::map<int, RobotGroup> &robot_groups,
127 const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());
154 virtual bool trajectory_to_msgs(
const motoman_msgs::DynamicJointTrajectoryConstPtr& traj,
155 std::vector<SimpleMessage>* msgs);
167 std::vector<SimpleMessage>* msgs);
178 virtual bool transform(
const trajectory_msgs::JointTrajectoryPoint& pt_in,
179 trajectory_msgs::JointTrajectoryPoint* pt_out)
185 virtual bool transform(
const motoman_msgs::DynamicJointsGroup& pt_in, motoman_msgs::DynamicJointsGroup* pt_out)
201 virtual bool select(
const std::vector<std::string>& ros_joint_names,
const motoman_msgs::DynamicJointsGroup& ros_pt,
202 const std::vector<std::string>& rbt_joint_names, motoman_msgs::DynamicJointsGroup* rbt_pt);
215 virtual bool select(
const std::vector<std::string>& ros_joint_names,
216 const trajectory_msgs::JointTrajectoryPoint& ros_pt,
217 const std::vector<std::string>& rbt_joint_names, trajectory_msgs::JointTrajectoryPoint* rbt_pt);
228 virtual bool create_message(
int seq,
const trajectory_msgs::JointTrajectoryPoint &pt, SimpleMessage* msg);
230 virtual bool create_message(
int seq,
const motoman_msgs::DynamicJointsGroup &pt, SimpleMessage* msg);
232 virtual bool create_message_ex(
int seq,
const motoman_msgs::DynamicJointPoint &pt, SimpleMessage* msg);
243 virtual bool calc_velocity(
const trajectory_msgs::JointTrajectoryPoint& pt,
double* rbt_velocity);
254 virtual bool calc_velocity(
const motoman_msgs::DynamicJointsGroup& pt,
double* rbt_velocity);
265 virtual bool calc_duration(
const trajectory_msgs::JointTrajectoryPoint& pt,
double* rbt_duration);
276 virtual bool calc_duration(
const motoman_msgs::DynamicJointsGroup& pt,
double* rbt_duration);
286 virtual bool send_to_robot(
const std::vector<SimpleMessage>& messages) = 0;
294 virtual void jointTrajectoryExCB(
const motoman_msgs::DynamicJointTrajectoryConstPtr &msg);
302 virtual void jointTrajectoryCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg);
312 virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req,
313 industrial_msgs::StopMotion::Response &res);
321 virtual bool is_valid(
const motoman_msgs::DynamicJointTrajectory &traj);
329 virtual bool is_valid(
const trajectory_msgs::JointTrajectory &traj);
336 virtual void jointStateCB(
const sensor_msgs::JointStateConstPtr &msg);
338 virtual void jointStateCB(
const sensor_msgs::JointStateConstPtr &msg,
int robot_id);
351 std::map<int, ros::ServiceServer> srv_stops_;
352 std::map<int, ros::ServiceServer> srv_joints_;
353 std::map<int, ros::Subscriber> sub_joint_trajectories_;
354 std::map<int, ros::Subscriber> sub_cur_positions_;
357 std::map<int, RobotGroup> robot_groups_;
365 std::map<int, sensor_msgs::JointState> cur_joint_pos_map_;
377 industrial_msgs::CmdJointTrajectory::Response &res);
380 bool jointTrajectoryExCB(motoman_msgs::CmdJointTrajectoryEx::Request &req,
381 motoman_msgs::CmdJointTrajectoryEx::Response &res);
387 #endif // MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_TRAJECTORY_INTERFACE_H ros::Subscriber sub_cur_pos_
virtual bool calc_velocity(const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_velocity)
ros::ServiceServer srv_joint_trajectory_
std::vector< std::string > all_joint_names_
virtual bool transform(const trajectory_msgs::JointTrajectoryPoint &pt_in, trajectory_msgs::JointTrajectoryPoint *pt_out)
virtual bool calc_duration(const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_duration)
SmplMsgConnection * connection_
sensor_msgs::JointState cur_joint_pos_
ros::ServiceServer srv_stop_motion_
virtual bool send_to_robot(const std::vector< JointTrajPtMessage > &messages)=0
ROSCPP_DECL void spin(Spinner &spinner)
double default_joint_pos_
static JointTrajPtMessage create_message(int seq, std::vector< double > joint_pos, double velocity, double duration)
double default_vel_ratio_
virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
virtual bool init(std::string default_ip="", int default_port=StandardSocketPorts::MOTION)
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)
virtual void jointStateCB(const sensor_msgs::JointStateConstPtr &msg)
std::map< std::string, double > joint_vel_limits_
virtual bool is_valid(const trajectory_msgs::JointTrajectory &traj)
virtual void trajectoryStop()
ros::Subscriber sub_joint_trajectory_
virtual bool stopMotionCB(industrial_msgs::StopMotion::Request &req, industrial_msgs::StopMotion::Response &res)
virtual ~JointTrajectoryInterface()
JointTrajectoryInterface()
TcpClient default_tcp_connection_
virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs)