42 namespace joint_trajectory_streamer
46 const std::map<std::string, double> &velocity_limits)
50 ROS_INFO(
"JointTrajectoryStreamer: init");
66 const std::map<std::string, double> &velocity_limits)
70 ROS_INFO(
"JointTrajectoryStreamer: init");
92 ROS_INFO(
"Receiving joint trajectory message");
100 if (msg->points.empty())
101 ROS_INFO(
"Empty trajectory received, canceling current trajectory");
103 ROS_ERROR(
"Trajectory splicing not yet implemented, stopping current motion.");
111 if (msg->points.empty())
113 ROS_INFO(
"Empty trajectory received while in IDLE state, nothing is done");
118 std::vector<SimpleMessage> new_traj_msgs;
128 ROS_INFO(
"Receiving joint trajectory message");
133 ROS_DEBUG(
"Current state is: %d", state);
136 if (msg->points.empty())
137 ROS_INFO(
"Empty trajectory received, canceling current trajectory");
139 ROS_ERROR(
"Trajectory splicing not yet implemented, stopping current motion.");
147 if (msg->points.empty())
149 ROS_INFO(
"Empty trajectory received while in IDLE state, nothing is done");
154 std::vector<SimpleMessage> new_traj_msgs;
164 ROS_INFO(
"Loading trajectory, setting state to streaming");
167 ROS_INFO(
"Executing trajectory of size: %d", static_cast<int>(messages.size()));
179 std::vector<SimpleMessage>* msgs)
190 msgs->push_back(msgs->back());
197 std::vector<SimpleMessage>* msgs)
208 msgs->push_back(msgs->back());
217 int connectRetryCount = 1;
219 ROS_INFO(
"Starting joint trajectory streamer thread");
225 if (connectRetryCount-- > 0)
227 ROS_INFO(
"Connecting to robot motion server");
232 connectRetryCount = 0;
233 else if (connectRetryCount <= 0)
235 ROS_ERROR(
"Timeout connecting to robot controller. Send new motion command to retry.");
243 SimpleMessage msg, tmpMsg, reply;
254 ROS_INFO(
"Trajectory streaming complete, setting state to IDLE");
261 ROS_DEBUG(
"Robot disconnected. Attempting reconnect...");
262 connectRetryCount = 5;
270 ROS_DEBUG(
"Sending joint trajectory point");
273 ROS_INFO(
"Point[%d of %d] sent to controller",
278 ROS_WARN(
"Failed sent joint point, will try again");
282 ROS_ERROR(
"Joint trajectory streamer: unknown state");
290 ROS_WARN(
"Exiting trajectory streamer thread");
297 ROS_DEBUG(
"Stop command sent, entering idle mode");
SmplMsgConnection * connection_
ros::Time streaming_start_
bool sendAndReceiveMsg(industrial::simple_message::SimpleMessage &send, industrial::simple_message::SimpleMessage &recv, bool verbose=false)
bool send_to_robot(const std::vector< JointTrajPtMessage > &messages)
virtual bool init(std::string default_ip="", int default_port=StandardSocketPorts::MOTION)
virtual void trajectoryStop()
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 >())
virtual bool isConnected()=0
virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
std::vector< JointTrajPtMessage > current_traj_
~JointTrajectoryStreamer()
boost::thread * streaming_thread_
virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs)
virtual bool trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs)
virtual bool makeConnect()=0