55#include <gtest/gtest.h>
57#include <tf2_eigen/tf2_eigen.hpp>
81 SCOPED_TRACE(
"testSimpleRequest");
87 pconfig_settings.
config = { {
"enforce_joint_model_state_space",
"0" } };
90 moveit_msgs::msg::MoveItErrorCodes error_code;
101 EXPECT_NE(pc->getOMPLSimpleSetup(),
nullptr);
107 EXPECT_TRUE(pc->getPathConstraints()->empty());
112 ASSERT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
117 SCOPED_TRACE(
"testPathConstraints");
125 pconfig_settings.
config = { {
"enforce_joint_model_state_space",
"0" },
126 {
"projection_evaluator",
"joints(" + joint_names[0] +
"," + joint_names[1] +
")" },
127 {
"type",
"geometric::PRM" } };
130 moveit_msgs::msg::MoveItErrorCodes error_code;
134 request.allowed_planning_time = 10.0;
139 geometry_msgs::msg::Quaternion ee_orientation = tf2::toMsg(Eigen::Quaterniond(ee_pose.rotation()));
152 EXPECT_NE(pc->getOMPLSimpleSetup(),
nullptr);
159 ASSERT_TRUE(response.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
162 auto path_constraints = pc->getPathConstraints();
163 EXPECT_FALSE(path_constraints->empty());
164 EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1u);
165 EXPECT_TRUE(path_constraints->getPositionConstraints().empty());
166 EXPECT_TRUE(path_constraints->getJointConstraints().empty());
167 EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty());
172 for (
const robot_trajectory::RobotTrajectoryPtr& trajectory : response.
trajectory)
174 for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
176 EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
182 request.path_constraints.orientation_constraints.clear();
184 { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 }));
189 EXPECT_NE(pc->getOMPLSimpleSetup(),
nullptr);
196 pc->solve(response2);
197 ASSERT_TRUE(response2.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
200 path_constraints = pc->getPathConstraints();
201 EXPECT_FALSE(path_constraints->empty());
202 EXPECT_EQ(path_constraints->getPositionConstraints().size(), 1u);
203 EXPECT_TRUE(path_constraints->getOrientationConstraints().empty());
208 for (
const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.
trajectory)
210 for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
212 EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
227 const std::vector<double>& goal)
const
231 request.allowed_planning_time = 5.0;
244 moveit_msgs::msg::Constraints joint_goal =
247 request.goal_constraints.push_back(joint_goal);
254 std::array<double, 3> dimensions)
256 shape_msgs::msg::SolidPrimitive box;
257 box.type = shape_msgs::msg::SolidPrimitive::BOX;
258 box.dimensions.resize(3);
259 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0];
260 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1];
261 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2];
263 geometry_msgs::msg::Pose box_pose;
264 box_pose.position.x = position[0];
265 box_pose.position.y = position[1];
266 box_pose.position.z = position[2];
267 box_pose.orientation.w = 1.0;
269 moveit_msgs::msg::PositionConstraint pc;
272 pc.constraint_region.primitives.push_back(box);
273 pc.constraint_region.primitive_poses.push_back(box_pose);
279 moveit_msgs::msg::OrientationConstraint
282 moveit_msgs::msg::OrientationConstraint oc;
285 oc.orientation = nominal_orientation;
286 oc.absolute_x_axis_tolerance = 0.3;
287 oc.absolute_y_axis_tolerance = 0.3;
288 oc.absolute_z_axis_tolerance = 0.3;
324 testSimpleRequest({ 0., -0.785, 0., -2.356, 0, 1.571, 0.785 }, { 0., -0.785, 0., -2.356, 0, 1.571, 0.685 });
346 testSimpleRequest({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
351 testPathConstraints({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
359 testing::InitGoogleTest(&argc, argv);
360 rclcpp::init(argc, argv);
362 const int ret = RUN_ALL_TESTS();
FanucTestPlanningContext()
PandaTestPlanningContext()
Generic implementation of the tests that can be executed on different robots.
TestPlanningContext(const std::string &robot_name, const std::string &group_name)
planning_interface::MotionPlanRequest createRequest(const std::vector< double > &start, const std::vector< double > &goal) const
moveit_msgs::msg::OrientationConstraint createOrientationConstraint(const geometry_msgs::msg::Quaternion &nominal_orientation)
Helper function to create a orientation constraint.
moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array< double, 3 > position, std::array< double, 3 > dimensions)
Helper function to create a position constraint.
ompl_interface::ModelBasedStateSpacePtr state_space_
rclcpp::Node::SharedPtr node_
void testSimpleRequest(const std::vector< double > &start, const std::vector< double > &goal)
void testPathConstraints(const std::vector< double > &start, const std::vector< double > &goal)
planning_scene::PlanningScenePtr planning_scene_
ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
ompl_interface::ModelBasedPlanningContextPtr planning_context_
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
State space to be used in combination with OMPL's constrained planning functionality.
ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, moveit_msgs::msg::MoveItErrorCodes &error_code, const rclcpp::Node::SharedPtr &node, bool use_constraints_approximations) const
Returns a planning context to OMPLInterface, which in turn passes it to OMPLPlannerManager.
void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
Specify configurations for the planners.
Robot independent test class setup.
moveit::core::RobotStatePtr robot_state_
moveit::core::RobotModelPtr robot_model_
std::string ee_link_name_
LoadTestRobot(const std::string &robot_name, const std::string &group_name)
const moveit::core::JointModelGroup * joint_model_group_
std::string base_link_name_
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
moveit_msgs::msg::MoveItErrorCodes error_code
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory
Specify the settings for a particular planning algorithm, for a particular group. The Planner plugin ...
std::map< std::string, std::string > config
Key-value pairs of settings that get passed to the planning algorithm.
std::string group
The group (as defined in the SRDF) this configuration is meant for.
int main(int argc, char **argv)
TEST_F(PandaTestPlanningContext, testSimpleRequest)