speed_observer.h
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 #ifndef SPEED_OBSERVER_H
19 #define SPEED_OBSERVER_H
20 
21 #include <atomic>
22 #include <map>
23 #include <vector>
24 
25 #include <ros/node_handle.h>
28 
29 #include <prbt_hardware_support/FrameSpeeds.h>
30 #include <prbt_hardware_support/SetSpeedLimit.h>
31 
32 namespace prbt_hardware_support
33 {
35 static constexpr unsigned int DEFAULT_ALLOWED_MISSED_CALCULATIONS{3};
36 
38 {
39 public:
46  SpeedObserver(ros::NodeHandle& nh, std::string& reference_frame, std::vector<std::string>& frames_to_observe);
47 
48 public:
54  void startObserving(const double frequency, const unsigned int allowed_missed_calculations = DEFAULT_ALLOWED_MISSED_CALCULATIONS);
55 
62  bool setSpeedLimitCb(SetSpeedLimit::Request& req, SetSpeedLimit::Response& res);
63 
67  void terminateNow();
68 
69 private:
73  void triggerStop1();
74 
80  FrameSpeeds createFrameSpeedsMessage(const std::map<std::string, double>& speeds) const;
81 
89  void waitUntillCanTransform(const std::string& frame, const ros::Time& time,
90  const unsigned short int max_num_retries = 10) const;
91 
98  std::pair<tf2::Vector3, ros::Time> getLatestPose(const std::string& frame) const;
99 
106  bool isWithinLimit(const double& speed) const;
107 
108 private:
117  static double speedFromTwoPoses(const tf2::Vector3& a, const tf2::Vector3& b, const double& t);
118 
119 private:
129 
131  const std::string reference_frame_;
133  const std::vector<std::string> frames_to_observe_;
135  std::atomic_bool terminate_{ false };
139  std::map<std::string, unsigned int> missed_calculations_;
140 
141 private:
143  static constexpr double DEFAULT_SPEED_LIMIT{ .25 };
145  static constexpr uint32_t DEFAULT_QUEUE_SIZE{ 10 };
147  static constexpr double WAITING_TIME_FOR_TRANSFORM_S{ 0.1 };
149  static constexpr double TIME_INTERVAL_EPSILON_S{ 1e-9 };
150 };
151 
152 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
153 
154 inline double SpeedObserver::speedFromTwoPoses(const tf2::Vector3& a, const tf2::Vector3& b, const double& t)
155 {
156  ROS_ASSERT(t != 0);
157  double d = tf2::tf2Distance(a, b);
158  return d / t;
159 }
160 
161 inline bool SpeedObserver::isWithinLimit(const double& speed) const
162 {
163  return speed < current_speed_limit_;
164 }
165 
167 {
168  ROS_DEBUG("terminateNow");
169  terminate_ = true;
170 }
171 
172 } // namespace prbt_hardware_support
173 
174 #endif // SPEED_OBSERVER_H
d
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.
void terminateNow()
This method will terminate the observation cycle started by startObserving().
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.
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...
geometry_msgs::TransformStamped t
tf2_ros::Buffer tf_buffer_
Needed to receive tf2 transformations over the wire, see https://wiki.ros.org/tf2/Tutorials/.
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...
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.
static constexpr double DEFAULT_SPEED_LIMIT
Speed limit to be set at launch.
const std::string reference_frame_
Reference frame for all speeds.
const std::vector< std::string > frames_to_observe_
All frames to observe.
bool setSpeedLimitCb(SetSpeedLimit::Request &req, SetSpeedLimit::Response &res)
Callback for service to set the currently active speed limit.
TF2SIMD_FORCE_INLINE tf2Scalar tf2Distance(const Vector3 &v1, const Vector3 &v2)
static constexpr unsigned int DEFAULT_ALLOWED_MISSED_CALCULATIONS
Allowed number of missed calculations. If it is exceeded a Stop1 is triggered.
ros::ServiceClient sto_client_
Client for sto service.
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.
#define ROS_ASSERT(cond)
tf2_ros::TransformListener tf_listener
Listing to TF Transforms.
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.
#define ROS_DEBUG(...)


prbt_hardware_support
Author(s):
autogenerated on Fri Feb 28 2020 03:16:57