joint_trajectory_streamer.cpp
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 
33 
35 
37 {
38 namespace joint_trajectory_streamer
39 {
40 
41 bool JointTrajectoryStreamer::init(SmplMsgConnection* connection, const std::vector<std::string> &joint_names,
42  const std::map<std::string, double> &velocity_limits)
43 {
44  bool rtn = true;
45 
46  ROS_INFO("JointTrajectoryStreamer: init");
47 
48  rtn &= JointTrajectoryInterface::init(connection, joint_names, velocity_limits);
49 
50  this->mutex_.lock();
51  this->current_point_ = 0;
53  this->streaming_thread_ =
54  new boost::thread(boost::bind(&JointTrajectoryStreamer::streamingThread, this));
55  ROS_INFO("Unlocking mutex");
56  this->mutex_.unlock();
57 
58  return rtn;
59 }
60 
62 {
63  delete this->streaming_thread_;
64 }
65 
66 void JointTrajectoryStreamer::jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
67 {
68  ROS_INFO("Receiving joint trajectory message");
69 
70  // read current state value (should be atomic)
71  const auto state = this->state_;
72 
73  ROS_DEBUG("Current state is: %d", state);
74 
75  // always request a stop of current trajectory execution if an empty trajectory
76  // is received. We handle this separately from the check below, as the server
77  // might be executing a trajectory which this client has already finished
78  // uploading (due to buffering on the server side fi), and then our local state
79  // would be "IDLE", and we'd end up not sending the stop request.
80  if (msg->points.empty())
81  {
82  ROS_INFO_STREAM("Empty trajectory received while in state: " << TransferStates::to_string(state) << ". Canceling current trajectory.");
83  this->mutex_.lock();
85  this->mutex_.unlock();
86  return;
87  }
88 
89  // if we're currently streaming a trajectory and we're requested to stream another
90  // we complain, as splicing is not supported. Cancellation of the current trajectory
91  // should first be requested, then a new trajectory started.
92  if (TransferStates::IDLE != state)
93  {
94  ROS_ERROR("Trajectory splicing not yet implemented, stopping current motion.");
95  this->mutex_.lock();
97  this->mutex_.unlock();
98  return;
99  }
100 
101  // calc new trajectory
102  std::vector<JointTrajPtMessage> new_traj_msgs;
103  if (!trajectory_to_msgs(msg, &new_traj_msgs))
104  return;
105 
106  // send command messages to robot
107  send_to_robot(new_traj_msgs);
108 }
109 
110 bool JointTrajectoryStreamer::send_to_robot(const std::vector<JointTrajPtMessage>& messages)
111 {
112  ROS_INFO("Loading trajectory, setting state to streaming");
113  this->mutex_.lock();
114  {
115  ROS_INFO("Executing trajectory of size: %d", (int)messages.size());
116  this->current_traj_ = messages;
117  this->current_point_ = 0;
120  }
121  this->mutex_.unlock();
122 
123  return true;
124 }
125 
126 bool JointTrajectoryStreamer::trajectory_to_msgs(const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector<JointTrajPtMessage>* msgs)
127 {
128  // use base function to transform points
130  return false;
131 
132  // pad trajectory as required for minimum streaming buffer size
133  if (!msgs->empty() && (msgs->size() < (size_t)min_buffer_size_))
134  {
135  ROS_DEBUG("Padding trajectory: current(%d) => minimum(%d)", (int)msgs->size(), min_buffer_size_);
136  while (msgs->size() < (size_t)min_buffer_size_)
137  msgs->push_back(msgs->back());
138  }
139 
140  return true;
141 }
142 
143 
145 {
146  JointTrajPtMessage jtpMsg;
147  int connectRetryCount = 1;
148 
149  ROS_INFO("Starting joint trajectory streamer thread");
150  while (ros::ok())
151  {
152  ros::Duration(0.005).sleep();
153 
154  // automatically re-establish connection, if required
155  if (connectRetryCount-- > 0)
156  {
157  ROS_INFO("Connecting to robot motion server");
158  this->connection_->makeConnect();
159  ros::Duration(0.250).sleep(); // wait for connection
160 
161  if (this->connection_->isConnected())
162  connectRetryCount = 0;
163  else if (connectRetryCount <= 0)
164  {
165  ROS_ERROR("Timeout connecting to robot controller. Send new motion command to retry.");
167  }
168  continue;
169  }
170 
171  this->mutex_.lock();
172 
173  SimpleMessage msg, reply;
174 
175  switch (this->state_)
176  {
178  ros::Duration(0.010).sleep(); // loop while waiting for new trajectory
179  break;
180 
182  if (this->current_point_ >= (int)this->current_traj_.size())
183  {
184  ROS_INFO("Trajectory streaming complete, setting state to IDLE");
186  break;
187  }
188 
189  if (!this->connection_->isConnected())
190  {
191  ROS_DEBUG("Robot disconnected. Attempting reconnect...");
192  connectRetryCount = 5;
193  break;
194  }
195 
196  jtpMsg = this->current_traj_[this->current_point_];
197  jtpMsg.toRequest(msg);
198 
199  ROS_DEBUG("Sending joint trajectory point");
200  if (this->connection_->sendAndReceiveMsg(msg, reply, false))
201  {
202  this->current_point_++;
203  ROS_INFO("Point[%d of %d] sent to controller",
204  this->current_point_, (int)this->current_traj_.size());
205  }
206  else
207  ROS_WARN("Failed sent joint point, will try again");
208 
209  break;
210  default:
211  ROS_ERROR("Joint trajectory streamer: unknown state");
213  break;
214  }
215 
216  this->mutex_.unlock();
217  }
218 
219  ROS_WARN("Exiting trajectory streamer thread");
220 }
221 
223 {
225 
226  ROS_DEBUG("Stop command sent, entering idle mode");
228 }
229 
230 } //joint_trajectory_streamer
231 } //industrial_robot_client
232 
#define ROS_ERROR(...)
#define ROS_WARN(...)
virtual bool toRequest(industrial::simple_message::SimpleMessage &msg)
#define ROS_DEBUG(...)
bool sendAndReceiveMsg(industrial::simple_message::SimpleMessage &send, industrial::simple_message::SimpleMessage &recv, bool verbose=false)
bool send_to_robot(const std::vector< JointTrajPtMessage > &messages)
Send trajectory to robot, using this node&#39;s robot-connection. Specific method must be implemented in ...
#define ROS_INFO(...)
virtual bool init(std::string default_ip="", int default_port=StandardSocketPorts::MOTION)
Initialize robot connection using default method.
ROSCPP_DECL bool ok()
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 >())
Class initializer.
virtual void jointTrajectoryCB(const trajectory_msgs::JointTrajectoryConstPtr &msg)
Callback function registered to ROS topic-subscribe. Transform message into SimpleMessage objects and...
#define ROS_INFO_STREAM(args)
static Time now()
bool sleep() const
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. Also includes various joint transforms that can be overridden for robot-specific behavior.
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. Also includes various joint transforms that can be overridden for robot-specific behavior.


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Mon Feb 28 2022 22:34:42