moveit2
The MoveIt Motion Planning Framework for ROS 2.
trajectory_monitor.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <limits>
41 #include <memory>
42 #include <moveit/utils/logger.hpp>
43 
44 planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor,
45  double sampling_frequency)
46  : TrajectoryMonitor(state_monitor, std::make_unique<TrajectoryMonitorMiddlewareHandle>(sampling_frequency),
47  sampling_frequency)
48 {
49 }
50 
52  const CurrentStateMonitorConstPtr& state_monitor,
53  std::unique_ptr<TrajectoryMonitor::MiddlewareHandle> middleware_handle, double sampling_frequency)
54  : current_state_monitor_(state_monitor)
55  , middleware_handle_(std::move(middleware_handle))
56  , sampling_frequency_(sampling_frequency)
57  , trajectory_(current_state_monitor_->getRobotModel(), "")
58  , logger_(moveit::getLogger("trajectory_monitor"))
59 {
60  setSamplingFrequency(sampling_frequency);
61 }
62 
64 {
65  stopTrajectoryMonitor();
66 }
67 
69 {
70  if (sampling_frequency == sampling_frequency_)
71  return; // silently return if nothing changes
72 
73  if (sampling_frequency <= std::numeric_limits<double>::epsilon())
74  {
75  RCLCPP_ERROR(logger_, "The sampling frequency for trajectory states should be positive");
76  }
77  else
78  {
79  RCLCPP_DEBUG(logger_, "Setting trajectory sampling frequency to %.1f", sampling_frequency);
80  }
81  sampling_frequency_ = sampling_frequency;
82 }
83 
85 {
86  return static_cast<bool>(record_states_thread_);
87 }
88 
90 {
91  if (sampling_frequency_ > std::numeric_limits<double>::epsilon() && !record_states_thread_)
92  {
93  record_states_thread_ = std::make_unique<std::thread>([this] { recordStates(); });
94  RCLCPP_DEBUG(logger_, "Started trajectory monitor");
95  }
96 }
97 
99 {
100  if (record_states_thread_)
101  {
102  std::unique_ptr<std::thread> copy;
103  copy.swap(record_states_thread_);
104  copy->join();
105  RCLCPP_DEBUG(logger_, "Stopped trajectory monitor");
106  }
107 }
108 
110 {
111  bool restart = isActive();
112  if (restart)
113  stopTrajectoryMonitor();
114  trajectory_.clear();
115  if (restart)
116  startTrajectoryMonitor();
117 }
118 
119 void planning_scene_monitor::TrajectoryMonitor::recordStates()
120 {
121  if (!current_state_monitor_)
122  return;
123 
124  middleware_handle_->setRate(sampling_frequency_);
125 
126  while (record_states_thread_)
127  {
128  middleware_handle_->sleep();
129  std::pair<moveit::core::RobotStatePtr, rclcpp::Time> state = current_state_monitor_->getCurrentStateAndTime();
130  if (trajectory_.empty())
131  {
132  trajectory_.addSuffixWayPoint(state.first, 0.0);
133  trajectory_start_time_ = state.second;
134  last_recorded_state_time_ = state.second;
135  }
136  else
137  {
138  trajectory_.addSuffixWayPoint(state.first, (state.second - last_recorded_state_time_).seconds());
139  last_recorded_state_time_ = state.second;
140  }
141  if (state_add_callback_)
142  state_add_callback_(state.first, state.second);
143  }
144 }
This class contains the ROS2 interfaces for TrajectoryMonitor. This class is useful for testing by mo...
Monitors the joint_states topic and tf to record the trajectory of the robot.
TrajectoryMonitor(const CurrentStateMonitorConstPtr &state_monitor, double sampling_frequency=0.0)
Constructor.
void setSamplingFrequency(double sampling_frequency)
Main namespace for MoveIt.
Definition: exceptions.h:43