SMACC2
cb_move_joints.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
21#include <future>
23
25{
26CbMoveJoints::CbMoveJoints(const std::map<std::string, double> & jointValueTarget)
27: jointValueTarget_(jointValueTarget)
28{
29}
30
32
34{
36
37 if (this->group_)
38 {
39 moveit::planning_interface::MoveGroupInterface move_group(
40 getNode(), moveit::planning_interface::MoveGroupInterface::Options(*(this->group_)));
41 this->moveJoints(move_group);
42 }
43 else
44 {
46 }
47}
48
50 moveit::planning_interface::MoveGroupInterface & moveGroupInterface,
51 std::map<std::string, double> & targetJoints)
52{
53 auto state = moveGroupInterface.getCurrentState();
54
55 if (state == nullptr) return std::string();
56
57 auto vnames = state->getVariableNames();
58
59 std::stringstream ss;
60
61 for (auto & tgj : targetJoints)
62 {
63 auto it = std::find(vnames.begin(), vnames.end(), tgj.first);
64 auto index = std::distance(vnames.begin(), it);
65
66 ss << tgj.first << ":" << state->getVariablePosition(index) << std::endl;
67 }
68
69 return ss.str();
70}
71
72void CbMoveJoints::moveJoints(moveit::planning_interface::MoveGroupInterface & moveGroupInterface)
73{
74 if (scalingFactor_) moveGroupInterface.setMaxVelocityScalingFactor(*scalingFactor_);
75
76 bool success;
77 moveit::planning_interface::MoveGroupInterface::Plan computedMotionPlan;
78
79 if (jointValueTarget_.size() == 0)
80 {
81 RCLCPP_WARN(
82 getLogger(), "[CbMoveJoints] No joint was value specified. Skipping planning call.");
83 success = false;
84 }
85 else
86 {
87 moveGroupInterface.setJointValueTarget(jointValueTarget_);
88 //moveGroupInterface.setGoalJointTolerance(0.01);
89 success =
90 (moveGroupInterface.plan(computedMotionPlan) ==
91 moveit::planning_interface::MoveItErrorCode::SUCCESS);
92 RCLCPP_INFO(getLogger(), "Success Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
93 }
94
95 if (success)
96 {
97 auto executionResult = moveGroupInterface.execute(computedMotionPlan);
98
99 auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_);
100
101 if (executionResult == moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
102 {
103 RCLCPP_INFO_STREAM(
104 getLogger(), "[" << this->getName()
105 << "] motion execution succeeded. Throwing success event. " << std::endl
106 << statestr);
108 this->postSuccessEvent();
109 }
110 else
111 {
112 RCLCPP_WARN_STREAM(
113 getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."
114 << std::endl
115 << statestr);
117 this->postFailureEvent();
118 }
119 }
120 else
121 {
122 auto statestr = currentJointStatesToString(moveGroupInterface, jointValueTarget_);
123 RCLCPP_WARN_STREAM(
124 getLogger(), "[" << this->getName() << "] motion execution failed. Throwing fail event."
125 << std::endl
126 << statestr);
128 this->postFailureEvent();
129 }
130}
131
133} // namespace cl_move_group_interface
std::optional< std::string > group_
std::optional< double > scalingFactor_
std::map< std::string, double > jointValueTarget_
void moveJoints(moveit::planning_interface::MoveGroupInterface &moveGroupInterface)
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Node::SharedPtr getNode()
void requiresClient(SmaccClientType *&storage)
std::string currentJointStatesToString(moveit::planning_interface::MoveGroupInterface &moveGroupInterface, std::map< std::string, double > &targetJoints)