moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
trajectory_monitor.h
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
37#pragma once
38
43#include <rclcpp/time.hpp>
44#include <memory>
45#include <functional>
46#include <thread>
47
49{
50using TrajectoryStateAddedCallback = std::function<void(const moveit::core::RobotStateConstPtr&, const rclcpp::Time&)>;
51
52MOVEIT_CLASS_FORWARD(TrajectoryMonitor); // Defines TrajectoryMonitorPtr, ConstPtr, WeakPtr... etc
53
57{
58public:
63 {
64 public:
68 virtual ~MiddlewareHandle() = default;
69
73 virtual void setRate(double sampling_frequency) = 0;
74
78 virtual void sleep() = 0;
79 };
80
85 TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor, double sampling_frequency = 0.0);
86
91 TrajectoryMonitor(const CurrentStateMonitorConstPtr& state_monitor,
92 std::unique_ptr<MiddlewareHandle> middleware_handle, double sampling_frequency = 0.0);
93
95
97
99
100 void clearTrajectory();
101
102 bool isActive() const;
103
104 double getSamplingFrequency() const
105 {
106 return sampling_frequency_;
107 }
108
109 void setSamplingFrequency(double sampling_frequency);
110
114 {
115 return trajectory_;
116 }
117
119 {
120 trajectory_.swap(other);
121 }
122
124 {
125 state_add_callback_ = callback;
126 }
127
128private:
129 void recordStates();
130
131 // Samples robot states.
132 CurrentStateMonitorConstPtr current_state_monitor_;
133 // Interface for communicating with ROS.
134 std::unique_ptr<MiddlewareHandle> middleware_handle_;
135 double sampling_frequency_;
136
138 rclcpp::Time trajectory_start_time_;
139 rclcpp::Time last_recorded_state_time_;
140
141 std::unique_ptr<std::thread> record_states_thread_;
142 TrajectoryStateAddedCallback state_add_callback_;
143
144 rclcpp::Logger logger_;
145};
146} // namespace planning_scene_monitor
#define MOVEIT_CLASS_FORWARD(C)
This class contains the rcl interfaces for easier testing.
virtual void sleep()=0
Sleep the handle for some prescribed amount of time.
virtual void setRate(double sampling_frequency)=0
set Rate using sampling frequency
virtual ~MiddlewareHandle()=default
Destroys the object.
Monitors the joint_states topic and tf to record the trajectory of the robot.
void swapTrajectory(robot_trajectory::RobotTrajectory &other)
const robot_trajectory::RobotTrajectory & getTrajectory()
TrajectoryMonitor(const CurrentStateMonitorConstPtr &state_monitor, std::unique_ptr< MiddlewareHandle > middleware_handle, double sampling_frequency=0.0)
Constructor with middleware handle as input parameter.
void setSamplingFrequency(double sampling_frequency)
void setOnStateAddCallback(const TrajectoryStateAddedCallback &callback)
Maintain a sequence of waypoints and the time durations between these waypoints.
void swap(robot_trajectory::RobotTrajectory &other) noexcept
std::function< void(const moveit::core::RobotStateConstPtr &, const rclcpp::Time &)> TrajectoryStateAddedCallback