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());
111 ASSERT_TRUE(pc->solve(res));
116 SCOPED_TRACE(
"testPathConstraints");
124 pconfig_settings.
config = { {
"enforce_joint_model_state_space",
"0" },
125 {
"projection_evaluator",
"joints(" + joint_names[0] +
"," + joint_names[1] +
")" },
126 {
"type",
"geometric::PRM" } };
129 moveit_msgs::msg::MoveItErrorCodes error_code;
133 request.allowed_planning_time = 10.0;
138 geometry_msgs::msg::Quaternion ee_orientation = tf2::toMsg(Eigen::Quaterniond(ee_pose.rotation()));
151 EXPECT_NE(pc->getOMPLSimpleSetup(),
nullptr);
157 ASSERT_TRUE(pc->solve(response));
160 auto path_constraints = pc->getPathConstraints();
161 EXPECT_FALSE(path_constraints->empty());
162 EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1u);
163 EXPECT_TRUE(path_constraints->getPositionConstraints().empty());
164 EXPECT_TRUE(path_constraints->getJointConstraints().empty());
165 EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty());
170 for (
const robot_trajectory::RobotTrajectoryPtr& trajectory : response.
trajectory)
172 for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
174 EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
180 request.path_constraints.orientation_constraints.clear();
182 { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 }));
187 EXPECT_NE(pc->getOMPLSimpleSetup(),
nullptr);
194 ASSERT_TRUE(pc->solve(response2));
197 path_constraints = pc->getPathConstraints();
198 EXPECT_FALSE(path_constraints->empty());
199 EXPECT_EQ(path_constraints->getPositionConstraints().size(), 1u);
200 EXPECT_TRUE(path_constraints->getOrientationConstraints().empty());
205 for (
const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.
trajectory)
207 for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
209 EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
224 const std::vector<double>& goal)
const
228 request.allowed_planning_time = 5.0;
241 moveit_msgs::msg::Constraints joint_goal =
244 request.goal_constraints.push_back(joint_goal);
251 std::array<double, 3> dimensions)
253 shape_msgs::msg::SolidPrimitive box;
254 box.type = shape_msgs::msg::SolidPrimitive::BOX;
255 box.dimensions.resize(3);
256 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0];
257 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1];
258 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2];
260 geometry_msgs::msg::Pose box_pose;
261 box_pose.position.x = position[0];
262 box_pose.position.y = position[1];
263 box_pose.position.z = position[2];
264 box_pose.orientation.w = 1.0;
266 moveit_msgs::msg::PositionConstraint pc;
269 pc.constraint_region.primitives.push_back(box);
270 pc.constraint_region.primitive_poses.push_back(box_pose);
276 moveit_msgs::msg::OrientationConstraint
279 moveit_msgs::msg::OrientationConstraint oc;
282 oc.orientation = nominal_orientation;
283 oc.absolute_x_axis_tolerance = 0.3;
284 oc.absolute_y_axis_tolerance = 0.3;
285 oc.absolute_z_axis_tolerance = 0.3;
321 testSimpleRequest({ 0., -0.785, 0., -2.356, 0, 1.571, 0.785 }, { 0., -0.785, 0., -2.356, 0, 1.571, 0.685 });
343 testSimpleRequest({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
348 testPathConstraints({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
354 int main(
int argc,
char** argv)
356 testing::InitGoogleTest(&argc, argv);
357 rclcpp::init(argc, argv);
359 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.
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
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)