cartesian_trajectory_controller.h
Go to the documentation of this file.
1 // -- BEGIN LICENSE BLOCK ----------------------------------------------
2 // Copyright 2020 FZI Forschungszentrum Informatik
3 // Created on behalf of Universal Robots A/S
4 //
5 // Licensed under the Apache License, Version 2.0 (the "License");
6 // you may not use this file except in compliance with the License.
7 // You may obtain a copy of the License at
8 //
9 // http://www.apache.org/licenses/LICENSE-2.0
10 //
11 // Unless required by applicable law or agreed to in writing, software
12 // distributed under the License is distributed on an "AS IS" BASIS,
13 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 // See the License for the specific language governing permissions and
15 // limitations under the License.
16 // -- END LICENSE BLOCK ------------------------------------------------
17 
18 //-----------------------------------------------------------------------------
25 //-----------------------------------------------------------------------------
26 
27 #pragma once
28 
32 #include <cartesian_control_msgs/FollowCartesianTrajectoryAction.h>
33 #include <cartesian_control_msgs/CartesianTolerance.h>
34 #include <atomic>
35 #include <mutex>
38 
40 {
41 template <class HWInterface>
43 {
44 public:
46 
48 
49  bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
50 
51  void starting(const ros::Time& time) override;
52 
53  void stopping(const ros::Time& time) override;
54 
55  void update(const ros::Time& time, const ros::Duration& period) override;
56 
57  void executeCB(const cartesian_control_msgs::FollowCartesianTrajectoryGoalConstPtr& goal);
58 
59  void preemptCB();
60 
61 protected:
63 
65  {
66  TrajectoryDuration() : now(0.0), end(0.0)
67  {
68  }
69 
72  };
73 
74  void timesUp();
75 
77 
79  const cartesian_control_msgs::CartesianTolerance& tolerance);
80 
81 private:
82  std::unique_ptr<scaled_controllers::SpeedScalingHandle> speed_scaling_;
83  std::unique_ptr<actionlib::SimpleActionServer<cartesian_control_msgs::FollowCartesianTrajectoryAction> >
85  std::atomic<bool> done_;
86  std::mutex lock_;
89  cartesian_control_msgs::CartesianTolerance path_tolerances_;
90  cartesian_control_msgs::CartesianTolerance goal_tolerances_;
91 };
92 
93 } // namespace cartesian_trajectory_controller
94 
cartesian_trajectory.h
cartesian_trajectory_controller::CartesianTrajectoryController::lock_
std::mutex lock_
Definition: cartesian_trajectory_controller.h:86
control_policies.h
cartesian_trajectory_controller::CartesianTrajectoryController::speed_scaling_
std::unique_ptr< scaled_controllers::SpeedScalingHandle > speed_scaling_
Definition: cartesian_trajectory_controller.h:82
ros_controllers_cartesian
cartesian_trajectory_controller::CartesianTrajectoryController::executeCB
void executeCB(const cartesian_control_msgs::FollowCartesianTrajectoryGoalConstPtr &goal)
Definition: cartesian_trajectory_controller.hpp:131
cartesian_trajectory_controller::CartesianTrajectoryController::init
bool init(hardware_interface::RobotHW *hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
Definition: cartesian_trajectory_controller.hpp:33
cartesian_trajectory_controller
Definition: cartesian_trajectory_controller.h:39
cartesian_trajectory_controller::CartesianTrajectoryController::goal_tolerances_
cartesian_control_msgs::CartesianTolerance goal_tolerances_
Definition: cartesian_trajectory_controller.h:90
cartesian_trajectory_controller::CartesianTrajectoryController::timesUp
void timesUp()
Definition: cartesian_trajectory_controller.hpp:191
cartesian_trajectory_controller.hpp
simple_action_server.h
cartesian_trajectory_controller::CartesianTrajectoryController::CartesianTrajectoryController
CartesianTrajectoryController()
Definition: cartesian_trajectory_controller.h:45
cartesian_trajectory_controller::CartesianTrajectoryController::starting
void starting(const ros::Time &time) override
Definition: cartesian_trajectory_controller.hpp:71
ros_controllers_cartesian::ControlPolicy
Primary template.
Definition: control_policies.h:92
cartesian_trajectory_controller::CartesianTrajectoryController
Definition: cartesian_trajectory_controller.h:42
hardware_interface::RobotHW
speed_scaling_interface.h
ros_controllers_cartesian::CartesianState
cartesian_trajectory_controller::CartesianTrajectoryController::monitorExecution
void monitorExecution(const ros_controllers_cartesian::CartesianState &error)
Definition: cartesian_trajectory_controller.hpp:226
ros_controllers_cartesian::CartesianTrajectory
cartesian_trajectory_controller::CartesianTrajectoryController::trajectory_duration_
TrajectoryDuration trajectory_duration_
Definition: cartesian_trajectory_controller.h:88
cartesian_trajectory_controller::CartesianTrajectoryController::~CartesianTrajectoryController
virtual ~CartesianTrajectoryController()
Definition: cartesian_trajectory_controller.h:47
cartesian_state.h
cartesian_trajectory_controller::CartesianTrajectoryController::done_
std::atomic< bool > done_
Definition: cartesian_trajectory_controller.h:85
cartesian_trajectory_controller::CartesianTrajectoryController::action_server_
std::unique_ptr< actionlib::SimpleActionServer< cartesian_control_msgs::FollowCartesianTrajectoryAction > > action_server_
Definition: cartesian_trajectory_controller.h:84
cartesian_trajectory_controller::CartesianTrajectoryController::TrajectoryDuration::end
ros::Duration end
Planned target duration of the current action.
Definition: cartesian_trajectory_controller.h:70
cartesian_trajectory_controller::CartesianTrajectoryController::TrajectoryDuration::now
ros::Duration now
Current duration of the current action.
Definition: cartesian_trajectory_controller.h:71
ros::Time
cartesian_trajectory_controller::CartesianTrajectoryController::withinTolerances
bool withinTolerances(const ros_controllers_cartesian::CartesianState &error, const cartesian_control_msgs::CartesianTolerance &tolerance)
Definition: cartesian_trajectory_controller.hpp:240
cartesian_trajectory_controller::CartesianTrajectoryController::trajectory_
ros_controllers_cartesian::CartesianTrajectory trajectory_
Definition: cartesian_trajectory_controller.h:87
cartesian_trajectory_controller::CartesianTrajectoryController::preemptCB
void preemptCB()
Definition: cartesian_trajectory_controller.hpp:181
cartesian_trajectory_controller::CartesianTrajectoryController::stopping
void stopping(const ros::Time &time) override
Definition: cartesian_trajectory_controller.hpp:78
cartesian_trajectory_controller::CartesianTrajectoryController::path_tolerances_
cartesian_control_msgs::CartesianTolerance path_tolerances_
Definition: cartesian_trajectory_controller.h:89
cartesian_trajectory_controller::CartesianTrajectoryController::TrajectoryDuration::TrajectoryDuration
TrajectoryDuration()
Definition: cartesian_trajectory_controller.h:66
ros::Duration
cartesian_trajectory_controller::CartesianTrajectoryController::TrajectoryDuration
Definition: cartesian_trajectory_controller.h:64
ros::NodeHandle
cartesian_trajectory_controller::CartesianTrajectoryController::update
void update(const ros::Time &time, const ros::Duration &period) override
Definition: cartesian_trajectory_controller.hpp:88


cartesian_trajectory_controller
Author(s):
autogenerated on Tue Oct 15 2024 02:09:16