Go to the documentation of this file.
37 #include <gtest/gtest.h>
49 std::vector<std::size_t>& added_path_index)
const override
55 added_path_index.push_back(res.
trajectory_->getWayPointCount() - 1);
69 std::vector<std::size_t>& added_path_index)
const override
75 added_path_index.push_back(0);
88 planning_interface::PlanningContextPtr
91 moveit_msgs::MoveItErrorCodes& )
const override
162 for (std::size_t i = 0; i < waypointCount; ++i)
178 sut_.addAdapter(std::make_shared<AppendingPlanningRequestAdapter>());
179 sut_.addAdapter(std::make_shared<PrependingPlanningRequestAdapter>());
180 sut_.addAdapter(std::make_shared<AppendingPlanningRequestAdapter>());
182 planning_context_->setTrajectory(createRandomTrajectory(4));
185 req.group_name = group_name_;
187 std::vector<std::size_t> added_path_index;
188 sut_.adaptAndPlan(planner_manager_, planning_scene_, req, res, added_path_index);
190 ASSERT_EQ(added_path_index.size(), 3UL);
196 int main(
int argc,
char** argv)
198 testing::InitGoogleTest(&argc, argv);
199 return RUN_ALL_TESTS();
bool terminate() override
If solve() is running, terminate the computation. Return false if termination not possible....
bool canServiceRequest(planning_interface::MotionPlanRequest const &) const override
Determine whether this plugin instance is able to represent this planning request.
planning_request_adapter::PlanningRequestAdapterChain sut_
moveit::core::RobotModelPtr robot_model_
planning_scene::PlanningScenePtr planning_scene_
bool adaptAndPlan(PlannerFn const &planner, planning_scene::PlanningSceneConstPtr const &planning_scene, planning_interface::MotionPlanRequest const &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &added_path_index) const override
Adapt the planning request if needed, call the planner function planner and update the planning respo...
void setTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory)
Apply a sequence of adapters to a motion plan.
robot_trajectory::RobotTrajectoryPtr trajectory_
robot_trajectory::RobotTrajectoryPtr m_trajectory
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory_
std::shared_ptr< PlannerManagerStub > planner_manager_
void initialize(ros::NodeHandle const &) override
Initialize parameters using the passed NodeHandle if no initialization is needed, simply implement as...
planning_interface::PlanningContextPtr m_planningContext
PlannerManagerStub(planning_interface::PlanningContextPtr planningContext)
bool solve(planning_interface::MotionPlanResponse &res) override
Solve the motion planning problem and store the result in res. This function should not clear data st...
int main(int argc, char **argv)
void clear() override
Clear the data structures used by the planner.
planning_interface::PlanningContextPtr getPlanningContext(planning_scene::PlanningSceneConstPtr const &, planning_interface::MotionPlanRequest const &, moveit_msgs::MoveItErrorCodes &) const override
Construct a planning context given the current scene and a planning request. If a problem is encounte...
moveit_msgs::MotionPlanRequest MotionPlanRequest
std::shared_ptr< PlanningContextStub > planning_context_
TEST_F(PlanningRequestAdapterTests, testMergeAddedPathIndex)
boost::function< bool(const planning_scene::PlanningSceneConstPtr &, const planning_interface::MotionPlanRequest &, planning_interface::MotionPlanResponse &)> PlannerFn
PlanningContext(const std::string &name, const std::string &group)
Construct a planning context named name for the group group.
bool adaptAndPlan(PlannerFn const &planner, planning_scene::PlanningSceneConstPtr const &planning_scene, planning_interface::MotionPlanRequest const &req, planning_interface::MotionPlanResponse &res, std::vector< std::size_t > &added_path_index) const override
Adapt the planning request if needed, call the planner function planner and update the planning respo...
bool solve(planning_interface::MotionPlanDetailedResponse &res) override
Solve the motion planning problem and store the detailed result in res. This function should not clea...
void initialize(ros::NodeHandle const &) override
Initialize parameters using the passed NodeHandle if no initialization is needed, simply implement as...
Representation of a particular planning context – the planning scene and the request are known,...
This namespace includes the central class for representing planning scenes.
robot_trajectory::RobotTrajectoryPtr createRandomTrajectory(std::size_t waypointCount)
Base class for a MoveIt planner.
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
moveit_core
Author(s): Ioan Sucan
, Sachin Chitta , Acorn Pooley
autogenerated on Fri May 3 2024 02:28:41