SMACC2
cp_trajectory_history.cpp
Go to the documentation of this file.
1// Copyright 2021 RobosoftAI Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15/*****************************************************************************************************************
16 *
17 * Authors: Pablo Inigo Blasco, Brett Aldrich
18 *
19 ******************************************************************************************************************/
20
22
24{
26 int backIndex, moveit_msgs::msg::RobotTrajectory & trajectory)
27{
28 if (trajectoryHistory_.size() == 0)
29 {
30 RCLCPP_WARN_STREAM(
31 getLogger(), "[" << getName() << "] requested index: " << backIndex
32 << ", history size: " << trajectoryHistory_.size());
33 return false;
34 }
35
36 if (backIndex < 0)
37
38 {
39 backIndex = 0;
40 }
41 else if ((size_t)backIndex >= this->trajectoryHistory_.size())
42 {
43 RCLCPP_WARN_STREAM(
44 getLogger(), "[" << getName() << "] requested index: " << backIndex
45 << ", history size: " << trajectoryHistory_.size());
46 return false;
47 }
48
49 trajectory = this->trajectoryHistory_[this->trajectoryHistory_.size() - 1 - backIndex].trajectory;
50 return true;
51}
52
53bool CpTrajectoryHistory::getLastTrajectory(moveit_msgs::msg::RobotTrajectory & trajectory)
54{
55 return getLastTrajectory(-1, trajectory);
56}
57
59 std::string name, const moveit_msgs::msg::RobotTrajectory & trajectory,
60 moveit_msgs::msg::MoveItErrorCodes result)
61{
62 RCLCPP_INFO_STREAM(
63 getLogger(), "[" << getName() << "] adding a new trajectory to the history ( "
64 << trajectory.joint_trajectory.points.size() << " poses)");
65
67 this->trajectoryHistory_.push_back(entry);
68
69 auto & last = this->trajectoryHistory_.back();
70 last.trajectory = trajectory;
71 last.result = result;
72 last.name = name;
73}
74
75} // namespace cl_move_group_interface
bool getLastTrajectory(int backIndex, moveit_msgs::msg::RobotTrajectory &trajectory)
void pushTrajectory(std::string name, const moveit_msgs::msg::RobotTrajectory &trajectory, moveit_msgs::msg::MoveItErrorCodes result)
std::vector< TrajectoryHistoryEntry > trajectoryHistory_
rclcpp::Logger getLogger()
virtual std::string getName() const