planning_request_adapter_plugins
src
resolve_constraint_frames.cpp
Go to the documentation of this file.
1
/*********************************************************************
2
* Software License Agreement (BSD License)
3
*
4
* Copyright (c) 2019, Bielefeld University
5
* All rights reserved.
6
*
7
* Redistribution and use in source and binary forms, with or without
8
* modification, are permitted provided that the following conditions
9
* are met:
10
*
11
* * Redistributions of source code must retain the above copyright
12
* notice, this list of conditions and the following disclaimer.
13
* * Redistributions in binary form must reproduce the above
14
* copyright notice, this list of conditions and the following
15
* disclaimer in the documentation and/or other materials provided
16
* with the distribution.
17
* * Neither the name of Bielefeld University nor the names of its
18
* contributors may be used to endorse or promote products derived
19
* from this software without specific prior written permission.
20
*
21
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32
* POSSIBILITY OF SUCH DAMAGE.
33
*********************************************************************/
34
35
/* Author: Robert Haschke */
36
37
#include <
moveit/planning_request_adapter/planning_request_adapter.h
>
38
#include <
moveit/kinematic_constraints/utils.h
>
39
#include <
class_loader/class_loader.hpp
>
40
#include <
ros/ros.h
>
41
42
namespace
default_planner_request_adapters
43
{
44
class
ResolveConstraintFrames :
public
planning_request_adapter::PlanningRequestAdapter
45
{
46
public
:
47
ResolveConstraintFrames
() :
planning_request_adapter
::
PlanningRequestAdapter
()
48
{
49
}
50
51
void
initialize
(
const
ros::NodeHandle
&
/*nh*/
)
override
52
{
53
}
54
55
std::string
getDescription
()
const override
56
{
57
return
"Resolve constraint frames to robot links"
;
58
}
59
60
bool
adaptAndPlan
(
const
PlannerFn
& planner,
const
planning_scene::PlanningSceneConstPtr&
planning_scene
,
61
const
planning_interface::MotionPlanRequest
& req,
planning_interface::MotionPlanResponse
& res,
62
std::vector<std::size_t>&
/*added_path_index*/
)
const override
63
{
64
ROS_DEBUG
(
"Running '%s'"
,
getDescription
().c_str());
65
planning_interface::MotionPlanRequest
modified = req;
66
kinematic_constraints::resolveConstraintFrames
(
planning_scene
->getCurrentState(), modified.path_constraints);
67
for
(moveit_msgs::Constraints& constraint : modified.goal_constraints)
68
kinematic_constraints::resolveConstraintFrames
(
planning_scene
->getCurrentState(), constraint);
69
return
planner(
planning_scene
, modified, res);
70
}
71
};
72
73
}
// namespace default_planner_request_adapters
74
75
CLASS_LOADER_REGISTER_CLASS
(
default_planner_request_adapters::ResolveConstraintFrames
,
76
planning_request_adapter::PlanningRequestAdapter
);
default_planner_request_adapters::ResolveConstraintFrames::getDescription
std::string getDescription() const override
Definition:
resolve_constraint_frames.cpp:119
planning_interface::MotionPlanResponse
utils.h
planning_request_adapter.h
ros.h
kinematic_constraints::resolveConstraintFrames
bool resolveConstraintFrames(const moveit::core::RobotState &state, moveit_msgs::Constraints &constraints)
ROS_DEBUG
#define ROS_DEBUG(...)
planning_request_adapter::PlanningRequestAdapter
class_loader.hpp
planning_interface::MotionPlanRequest
moveit_msgs::MotionPlanRequest MotionPlanRequest
default_planner_request_adapters::ResolveConstraintFrames::initialize
void initialize(const ros::NodeHandle &) override
Definition:
resolve_constraint_frames.cpp:115
planning_request_adapter::PlanningRequestAdapter::PlanningRequestAdapter
PlanningRequestAdapter()
default_planner_request_adapters
Definition:
add_iterative_spline_parameterization.cpp:43
planning_request_adapter::PlanningRequestAdapter::PlannerFn
boost::function< bool(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &, planning_interface::MotionPlanResponse &)> PlannerFn
default_planner_request_adapters::ResolveConstraintFrames::ResolveConstraintFrames
ResolveConstraintFrames()
Definition:
resolve_constraint_frames.cpp:111
default_planner_request_adapters::ResolveConstraintFrames
Definition:
resolve_constraint_frames.cpp:76
planning_request_adapter
CLASS_LOADER_REGISTER_CLASS
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::ResolveConstraintFrames, planning_request_adapter::PlanningRequestAdapter)
planning_scene
default_planner_request_adapters::ResolveConstraintFrames::adaptAndPlan
bool adaptAndPlan(const PlannerFn &planner, const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &) const override
Definition:
resolve_constraint_frames.cpp:124
ros::NodeHandle
planning
Author(s): Ioan Sucan
, Sachin Chitta
autogenerated on Sat Apr 27 2024 02:26:04