speed_observer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <algorithm>
19 #include <functional>
20 #include <std_srvs/SetBool.h>
21 #include <stdexcept>
22 #include <tf2/convert.h>
23 
26 
27 namespace prbt_hardware_support
28 {
29 
30 static const std::string FRAME_SPEEDS_TOPIC_NAME{ "frame_speeds" };
31 static const std::string STO_SERVICE{ "safe_torque_off" };
32 
33 SpeedObserver::SpeedObserver(ros::NodeHandle& nh, std::string& reference_frame,
34  std::vector<std::string>& frames_to_observe)
35  : nh_(nh), reference_frame_(reference_frame), frames_to_observe_(frames_to_observe)
36 {
39  sto_client_ = nh.serviceClient<std_srvs::SetBool>(STO_SERVICE);
40 }
41 
42 void SpeedObserver::waitUntillCanTransform(const std::string& frame, const ros::Time& time,
43  const unsigned short int max_num_retries) const
44 {
45  unsigned short int retries{ 0 };
46 
47  while (!terminate_ && (retries < max_num_retries) && !tf_buffer_.canTransform(reference_frame_, frame, time))
48  {
49  ros::spinOnce();
50  if (retries > 0) // when trying for the first time, we do not warn the user
51  ROS_WARN("Waiting for transform %s -> %s", reference_frame_.c_str(), frame.c_str());
53  ++retries;
54  }
55 
56  if (retries >= max_num_retries)
57  {
58  ROS_ERROR("Waited for transform %s -> %s too long.", reference_frame_.c_str(), frame.c_str());
59  throw std::runtime_error("Exceeded maximum number of retries.");
60  }
61 
62  if (terminate_)
63  {
64  throw std::runtime_error("Terminate flag is true"); // LCOV_EXCL_LINE Flag only needed for tests, therefore, excluded from line coverage.
65  }
66 }
67 
68 std::pair<tf2::Vector3, ros::Time> SpeedObserver::getLatestPose(const std::string& frame) const
69 {
70  tf2::Vector3 v;
71  ros::Time t;
72 
73  geometry_msgs::TransformStamped transform{ tf_buffer_.lookupTransform(reference_frame_, frame, ros::Time(0)) };
74  tf2::fromMsg(transform.transform.translation, v);
75  t = transform.header.stamp;
76  return std::pair<tf2::Vector3, ros::Time>(v, t);
77 }
78 
79 void SpeedObserver::startObserving(const double frequency, const unsigned int allowed_missed_calculations)
80 {
81  std::map<std::string, tf2::Vector3> previous_poses;
82  std::map<std::string, ros::Time> previous_time_stamps;
83  for (const auto& frame : frames_to_observe_)
84  {
86 
87  auto pose_data = getLatestPose(frame);
88  previous_poses[frame] = pose_data.first;
89  previous_time_stamps[frame] = pose_data.second;
90  missed_calculations_[frame] = 0;
91  }
92 
93  ros::Rate r(frequency);
94  ROS_INFO("Observing at %.1fHz", frequency);
95 
96  tf2::Vector3 curr_pos;
97  std::map<std::string, double> speeds;
98 
99  // Starting the observer loop
100  while (ros::ok() && !terminate_)
101  {
102  speeds.clear();
103  for (const auto& frame : frames_to_observe_)
104  {
105  if (terminate_)
106  {
107  return; // LCOV_EXCL_LINE Flag only needed for tests, therefore, excluded from line coverage.
108  }
109 
110  const auto curr_pose_data = getLatestPose(frame);
111  const auto &curr_pose = curr_pose_data.first;
112  const auto &curr_time_stamp = curr_pose_data.second;
113 
114  if (std::abs((ros::Time::now() - curr_time_stamp).toSec()) > 2.0/frequency)
115  {
116  ROS_WARN_STREAM("Latest transform of frame " << frame << " is too old. ");
117  ++missed_calculations_[frame];
118  ROS_WARN_STREAM("Missed calculations for frame " << frame << ": " << missed_calculations_[frame]);
119  if (missed_calculations_[frame] > allowed_missed_calculations)
120  {
121  ROS_ERROR_STREAM("Could not compute frame speed for frame " << frame
122  << " for " << allowed_missed_calculations << " times."
123  << " Triggering Stop1.");
124  triggerStop1();
125  missed_calculations_[frame] = 0;
126  }
127  }
128  else
129  {
130  double curr_speed{0.0};
131  if ((curr_time_stamp - previous_time_stamps.at(frame)).toSec() > TIME_INTERVAL_EPSILON_S)
132  {
133  curr_speed = speedFromTwoPoses(previous_poses.at(frame),
134  curr_pose,
135  (curr_time_stamp - previous_time_stamps.at(frame)).toSec());
136  }
137  else
138  {
139  ROS_WARN("Time interval too small for speed computation.");
140  }
141  if (!isWithinLimit(curr_speed))
142  {
143  ROS_ERROR("Speed %.2f m/s of frame >%s< exceeds limit of %.2f m/s", curr_speed, frame.c_str(),
145  triggerStop1();
146  }
147 
148  speeds[frame] = curr_speed;
149  previous_poses[frame] = tf2::Vector3(curr_pose);
150  previous_time_stamps[frame] = curr_time_stamp;
151  }
152  }
154  ros::spinOnce();
155  if (!terminate_)
156  {
157  r.sleep();
158  }
159  }
160 }
161 
162 FrameSpeeds SpeedObserver::createFrameSpeedsMessage(const std::map<std::string, double>& speeds) const
163 {
164  static uint32_t seq{ 0 };
165  FrameSpeeds msg;
166  msg.header.frame_id = reference_frame_;
167  msg.header.seq = seq++;
168  msg.header.stamp = ros::Time::now();
169  for (const auto& s : speeds)
170  {
171  msg.name.push_back(s.first);
172  msg.speed.push_back(s.second);
173  }
174  return msg;
175 }
176 
178 {
179  std_srvs::SetBool sto_srv;
180  sto_srv.request.data = false;
181  bool call_success = sto_client_.call(sto_srv);
182  if (!call_success)
183  {
184  ROS_ERROR_STREAM("No success calling service: " << sto_client_.getService());
185  }
186  else if (!sto_srv.response.success)
187  {
188  ROS_ERROR_STREAM("Service: " << sto_client_.getService() << " failed with error message:\n"
189  << sto_srv.response.message);
190  }
191 }
192 
193 bool SpeedObserver::setSpeedLimitCb(SetSpeedLimit::Request& req, SetSpeedLimit::Response& res)
194 {
195  ROS_DEBUG_STREAM("setSpeedLimitCb " << req.speed_limit);
196  current_speed_limit_ = req.speed_limit;
197  return true;
198 }
199 
200 } // namespace prbt_hardware_support
msg
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::map< std::string, unsigned int > missed_calculations_
Map to store the number of successive missed frame transform calculations.
std::atomic_bool terminate_
Helper variable to stop observation cycle.
void triggerStop1()
Used to trigger stop of the robot when speed limit is exceeded.
XmlRpcServer s
FrameSpeeds createFrameSpeedsMessage(const std::map< std::string, double > &speeds) const
Creates a message to be sent to the &#39;frame_speeds&#39; topic.
std::pair< tf2::Vector3, ros::Time > getLatestPose(const std::string &frame) const
Using tf to get the latest Pose of a frame as tf::Vector3.
bool call(MReq &req, MRes &res)
void waitUntillCanTransform(const std::string &frame, const ros::Time &time, const unsigned short int max_num_retries=10) const
Helper method waiting until TF transformation is available.
SpeedObserver(ros::NodeHandle &nh, std::string &reference_frame, std::vector< std::string > &frames_to_observe)
Create an observer to observe the speed of a list of frames in reference to a reference frame...
#define ROS_WARN(...)
geometry_msgs::TransformStamped t
tf2_ros::Buffer tf_buffer_
Needed to receive tf2 transformations over the wire, see https://wiki.ros.org/tf2/Tutorials/.
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &target_time, const ros::Duration timeout, std::string *errstr=NULL) const
static double speedFromTwoPoses(const tf2::Vector3 &a, const tf2::Vector3 &b, const double &t)
Calculate the minimal speed that a trajectory between two poses had if the motion was performed withi...
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
void publish(const boost::shared_ptr< M > &message) const
static constexpr double TIME_INTERVAL_EPSILON_S
Epsilon prevents computation of speed for small time intervals.
static constexpr double WAITING_TIME_FOR_TRANSFORM_S
Waiting time for waitUntillCanTransform()
static constexpr uint32_t DEFAULT_QUEUE_SIZE
Default queue size for publisher.
#define ROS_INFO(...)
static void waitForService(const std::string service_name, const double retry_timeout=DEFAULT_RETRY_TIMEOUT, const double msg_output_period=DEFAULT_MSG_OUTPUT_PERIOD)
void fromMsg(const A &, B &b)
ROSCPP_DECL bool ok()
static const std::string STO_SERVICE
const std::string reference_frame_
Reference frame for all speeds.
const std::vector< std::string > frames_to_observe_
All frames to observe.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool setSpeedLimitCb(SetSpeedLimit::Request &req, SetSpeedLimit::Response &res)
Callback for service to set the currently active speed limit.
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
bool sleep()
static const std::string FRAME_SPEEDS_TOPIC_NAME
static Time now()
ros::ServiceClient sto_client_
Client for sto service.
std::string getService()
ros::Publisher frame_speeds_pub_
Publisher for frame speed message.
bool isWithinLimit(const double &speed) const
Check if a speed value is within the currently set limit.
bool sleep() const
#define ROS_ERROR_STREAM(args)
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
void startObserving(const double frequency, const unsigned int allowed_missed_calculations=DEFAULT_ALLOWED_MISSED_CALCULATIONS)
Starts the observation cycle. The function blocks until ros shuts down.
double current_speed_limit_
Currently active speed limit.


prbt_hardware_support
Author(s):
autogenerated on Thu Feb 13 2020 03:16:51