Generic implementation of the tests that can be executed on different robots. More...

Inheritance diagram for TestPlanningContext:

[legend]

Collaboration diagram for TestPlanningContext:

[legend]

Public Member Functions
	TestPlanningContext (const std::string &robot_name, const std::string &group_name)

void	testSimpleRequest (const std::vector< double > &start, const std::vector< double > &goal)

void	testPathConstraints (const std::vector< double > &start, const std::vector< double > &goal)

Protected Member Functions
void	SetUp () override

planning_interface::MotionPlanRequest	createRequest (const std::vector< double > &start, const std::vector< double > &goal) const

moveit_msgs::msg::PositionConstraint	createPositionConstraint (std::array< double, 3 > position, std::array< double, 3 > dimensions)
	Helper function to create a position constraint. More...

moveit_msgs::msg::OrientationConstraint	createOrientationConstraint (const geometry_msgs::msg::Quaternion &nominal_orientation)
	Helper function to create a orientation constraint. More...

Protected Member Functions inherited from ompl_interface_testing::LoadTestRobot
	LoadTestRobot (const std::string &robot_name, const std::string &group_name)

Eigen::VectorXd	getRandomState () const

Eigen::VectorXd	getDeterministicState () const
	Create a joint position vector with values 0.1, 0.2, 0.3, ... where the length depends on the number of joints in the robot. More...

Protected Attributes
ompl_interface::ModelBasedStateSpacePtr	state_space_

ompl_interface::ModelBasedPlanningContextSpecification	planning_context_spec_

ompl_interface::ModelBasedPlanningContextPtr	planning_context_

planning_scene::PlanningScenePtr	planning_scene_

constraint_samplers::ConstraintSamplerManagerPtr	constraint_sampler_manager_

rclcpp::Node::SharedPtr	node_

Protected Attributes inherited from ompl_interface_testing::LoadTestRobot
std::string	group_name_

std::string	robot_name_

moveit::core::RobotModelPtr	robot_model_

moveit::core::RobotStatePtr	robot_state_

const moveit::core::JointModelGroup *	joint_model_group_

std::size_t	num_dofs_

std::string	base_link_name_

std::string	ee_link_name_

Detailed Description

Generic implementation of the tests that can be executed on different robots.

Test the creation of a ModelBasedPlanningContext through the PlanningContextManager.

The tests use a default robot state as start state, and then moves the last joint 0.1 radians/meters to create a joint goal. This creates an extremely simple planning problem to test general mechanics of the interface.

In the test with path constraints, we create an orientation constraint around the start orientation of the end-effector, with a tolerance on the rotation larger than the change in the last joint of the goal state. This makes sure the path constraints are easy to satisfy.

TODO(jeroendm) I also tried something similar with position constraints, but get a segmentation fault that occurs in the 'geometric_shapes' package, in the method 'useDimensions' in 'bodies.h'. git permalink: https://github.com/ros-planning/geometric_shapes/blob/df0478870b8592ce789ee1919f3124058c4327d7/include/geometric_shapes/bodies.h#L196

Definition at line 70 of file test_planning_context_manager.cpp.

Constructor & Destructor Documentation

◆ TestPlanningContext()

TestPlanningContext::TestPlanningContext	(	const std::string &	robot_name,
		const std::string &	group_name
	)

inline

Definition at line 73 of file test_planning_context_manager.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ createOrientationConstraint()

moveit_msgs::msg::OrientationConstraint TestPlanningContext::createOrientationConstraint ( const geometry_msgs::msg::Quaternion & nominal_orientation )

inlineprotected

Helper function to create a orientation constraint.

Definition at line 280 of file test_planning_context_manager.cpp.

Here is the caller graph for this function:

◆ createPositionConstraint()

moveit_msgs::msg::PositionConstraint TestPlanningContext::createPositionConstraint	(	std::array< double, 3 >	position,
		std::array< double, 3 >	dimensions
	)

inlineprotected

Helper function to create a position constraint.

Definition at line 253 of file test_planning_context_manager.cpp.

Here is the caller graph for this function:

◆ createRequest()

planning_interface::MotionPlanRequest TestPlanningContext::createRequest	(	const std::vector< double > &	start,
		const std::vector< double > &	goal
	)		const

inlineprotected

Create a planning request to plan from a given start state to a joint space goal.

Definition at line 226 of file test_planning_context_manager.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ SetUp()

void TestPlanningContext::SetUp ( )

inlineoverrideprotected

Definition at line 218 of file test_planning_context_manager.cpp.

◆ testPathConstraints()

void TestPlanningContext::testPathConstraints	(	const std::vector< double > &	start,
		const std::vector< double > &	goal
	)

inline

Definition at line 115 of file test_planning_context_manager.cpp.

Here is the call graph for this function:

◆ testSimpleRequest()

void TestPlanningContext::testSimpleRequest	(	const std::vector< double > &	start,
		const std::vector< double > &	goal
	)

inline

Definition at line 79 of file test_planning_context_manager.cpp.

Here is the call graph for this function:

Member Data Documentation

◆ constraint_sampler_manager_

constraint_samplers::ConstraintSamplerManagerPtr TestPlanningContext::constraint_sampler_manager_

protected

Definition at line 298 of file test_planning_context_manager.cpp.

◆ node_

rclcpp::Node::SharedPtr TestPlanningContext::node_

protected

Ideally we add an IK plugin to the joint_model_group_ to test the PoseModel state space, using the pluginlib to load the default KDL plugin? we need a node handle to call getPlanningRequest, but it is never used, as we disable the 'use_constraints_approximation' option.

Definition at line 306 of file test_planning_context_manager.cpp.