29 #ifndef FSROBO_R_DRIVER_JOINT_TRAJECTORY_STREAMER_H 30 #define FSROBO_R_DRIVER_JOINT_TRAJECTORY_STREAMER_H 35 #include <boost/thread/thread.hpp> 43 #include "fsrobo_r_msgs/ExecuteRobotProgram.h" 50 namespace joint_trajectory_streamer
57 namespace TransferStates
94 virtual bool init(
SmplMsgConnection *connection,
const std::vector<std::string> &joint_names,
95 const std::map<std::string, double> &velocity_limits = std::map<std::string, double>());
97 virtual void jointTrajectoryCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg);
99 virtual bool trajectory_to_msgs(
const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<JointTrajPtMessage> *msgs);
101 void streamingThread();
103 bool send_to_robot(
const std::vector<JointTrajPtMessage> &messages);
106 void trajectoryStop();
119 bool executeRobotProgramCB(fsrobo_r_msgs::ExecuteRobotProgram::Request &req, fsrobo_r_msgs::ExecuteRobotProgram::Response &res);
FSRoboRJointTrajectoryStreamer(int min_buffer_size=1)
Default constructor.
Wrapper class to exeute program on robot controller.
virtual bool init(std::string default_ip="", int default_port=StandardSocketPorts::MOTION)
RobotProgramExecutor robot_program_executor_
ros::ServiceServer srv_execute_robot_program
ros::Time streaming_start_
std::vector< JointTrajPtMessage > current_traj_
boost::thread * streaming_thread_