SMACC2
cb_move_last_trajectory_initial_state.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
23
25{
27
29: backIndex_(backIndex)
30{
31}
32
34
36{
37 CpTrajectoryHistory * trajectoryHistory;
38 this->requiresComponent(trajectoryHistory);
39
40 if (trajectoryHistory != nullptr)
41 {
42 moveit_msgs::msg::RobotTrajectory trajectory;
43
44 bool trajectoryFound = trajectoryHistory->getLastTrajectory(backIndex_, trajectory);
45
46 if (trajectoryFound)
47 {
48 trajectory_msgs::msg::JointTrajectoryPoint & initialPoint =
49 trajectory.joint_trajectory.points.front();
50
51 std::stringstream ss;
52 for (size_t i = 0; i < trajectory.joint_trajectory.joint_names.size(); i++)
53 {
54 auto & name = trajectory.joint_trajectory.joint_names[i];
55
56 jointValueTarget_[name] = initialPoint.positions[i];
57 ss << name << ": " << jointValueTarget_[name] << std::endl;
58 }
59 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "]" << std::endl << ss.str());
60
61 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] move joint onEntry");
63 RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] move joint onEntry finished");
64 }
65 }
66
67 //call base OnEntry
68}
69} // namespace cl_move_group_interface
std::map< std::string, double > jointValueTarget_
bool getLastTrajectory(int backIndex, moveit_msgs::msg::RobotTrajectory &trajectory)
void requiresComponent(SmaccComponentType *&storage)