SMACC2
cb_attach_object.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{
26CbAttachObject::CbAttachObject(std::string targetObjectName) : targetObjectName_(targetObjectName)
27{
28}
29
31
33{
35 this->requiresClient(moveGroup);
36
38 this->requiresComponent(graspingComponent);
39
40 // auto cubepos = cubeinfo->pose_->toPoseStampedMsg();
41
42 moveit_msgs::msg::CollisionObject targetCollisionObject;
43
44 bool found = graspingComponent->getGraspingObject(targetObjectName_, targetCollisionObject);
45
46 if (found)
47 {
48 targetCollisionObject.operation = moveit_msgs::msg::CollisionObject::ADD;
49 targetCollisionObject.header.stamp = getNode()->now();
50
51 moveGroup->planningSceneInterface->applyCollisionObject(targetCollisionObject);
52 // collisionObjects.push_back(cubeCollision);
53
55 moveGroup->moveGroupClientInterface->attachObject(
56 targetObjectName_, graspingComponent->gripperLink_, graspingComponent->fingerTipNames);
57
58 RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Grasping objectfound. attach request.");
59
60 this->postSuccessEvent();
61 }
62 else
63 {
64 RCLCPP_WARN_STREAM(
65 getLogger(), "[" << getName() << "] Grasping object was not found. Ignoring attach request.");
66
67 this->postFailureEvent();
68 }
69}
70
72} // namespace cl_move_group_interface
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
std::shared_ptr< moveit::planning_interface::PlanningSceneInterface > planningSceneInterface
std::optional< std::string > currentAttachedObjectName
bool getGraspingObject(std::string name, moveit_msgs::msg::CollisionObject &object)
virtual rclcpp::Node::SharedPtr getNode()
void requiresClient(SmaccClientType *&storage)
void requiresComponent(SmaccComponentType *&storage)