diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h index 7a36ae39a0..7d87109ca8 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -68,13 +68,15 @@ class MoveitCpp planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC); nh.param(ns + "publish_planning_scene_topic", publish_planning_scene_topic, planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC); - }; + nh.param(ns + "wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0); + } std::string name; std::string robot_description; std::string joint_state_topic; std::string attached_collision_object_topic; std::string monitored_planning_scene_topic; std::string publish_planning_scene_topic; + double wait_for_initial_state_timeout; }; struct PlanningPipelineOptions { @@ -83,7 +85,7 @@ class MoveitCpp std::string ns = "planning_pipelines/"; nh.getParam(ns + "pipeline_names", pipeline_names); nh.getParam(ns + "namespace", parent_namespace); - }; + } std::vector pipeline_names; std::string parent_namespace; }; @@ -95,7 +97,7 @@ class MoveitCpp nh.getParam(ns + "planning_attempts", planning_attempts); nh.getParam(ns + "max_velocity_scaling_factor", max_velocity_scaling_factor); nh.getParam(ns + "max_acceleration_scaling_factor", max_acceleration_scaling_factor); - }; + } int planning_attempts; double planning_time; double max_velocity_scaling_factor; diff --git a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h index 8595763668..fb99033ecd 100644 --- a/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace moveit { @@ -54,6 +55,35 @@ class PlanningComponent public: MOVEIT_STRUCT_FORWARD(PlanSolution); + class MoveItErrorCode : public moveit_msgs::MoveItErrorCodes + { + public: + MoveItErrorCode() + { + val = 0; + } + MoveItErrorCode(int code) + { + val = code; + } + MoveItErrorCode(const moveit_msgs::MoveItErrorCodes& code) + { + val = code.val; + } + explicit operator bool() const + { + return val == moveit_msgs::MoveItErrorCodes::SUCCESS; + } + bool operator==(const int c) const + { + return val == c; + } + bool operator!=(const int c) const + { + return val != c; + } + }; + /// The representation of a plan solution struct PlanSolution { @@ -62,6 +92,14 @@ class PlanningComponent /// The trajectory of the robot (may not contain joints that are the same as for the start_state_) robot_trajectory::RobotTrajectoryPtr trajectory; + + /// Error code + MoveItErrorCode error_code; + + explicit operator bool() const + { + return bool(error_code); + } }; /// Planner parameters provided with the MotionPlanRequest @@ -133,21 +171,25 @@ class PlanningComponent /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ - bool plan(); + PlanSolution plan(); /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the * provided PlanRequestParameters. */ - bool plan(const PlanRequestParameters& parameters); + PlanSolution plan(const PlanRequestParameters& parameters); /** \brief Execute the latest computed solution trajectory computed by plan(). By default this function terminates * after the execution is complete. The execution can be run in background by setting blocking to false. */ bool execute(bool blocking = true); + /** \brief Return the last plan solution*/ + const PlanSolutionPtr getLastPlanSolution(); + private: // Core properties and instances ros::NodeHandle nh_; MoveitCppPtr moveit_cpp_; const std::string group_name_; - moveit::core::JointModelGroupConstPtr joint_model_group_; + // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources + const moveit::core::JointModelGroup* joint_model_group_; // Planning std::set planning_pipeline_names_; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp index 38699be121..8ec4eb5a32 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit.cpp @@ -147,12 +147,10 @@ bool MoveitCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) ros::Duration(0.5).sleep(); // when at 0.1, i believe sometimes vjoint not properly loaded // Wait for complete state to be recieved - // TODO(henningkayser): parameterize - double wait_for_complete_state_timeout = 10.0; - // Break early - if (wait_for_complete_state_timeout > 0.0) + if (opt.wait_for_initial_state_timeout > 0.0) { - return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(); + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), + opt.wait_for_initial_state_timeout); } return true; diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp index 620a113f8e..e490f81694 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -73,7 +73,7 @@ constexpr char LOGNAME[] = "planning_component"; PlanningComponent::PlanningComponent(const std::string& group_name, const MoveitCppPtr& moveit_context) : group_name_(group_name), nh_(moveit_context->getNodeHandle()), moveit_cpp_(moveit_context) { - joint_model_group_.reset(moveit_cpp_->getRobotModel()->getJointModelGroup(group_name)); + joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); if (!joint_model_group_) { std::string error = "Could not find joint model group '" + group_name + "'."; @@ -126,12 +126,14 @@ const std::string& PlanningComponent::getName() const return group_name_; } -bool PlanningComponent::plan(const PlanRequestParameters& parameters) +PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParameters& parameters) { + last_plan_solution_.reset(new PlanSolution()); if (!joint_model_group_) { ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - return false; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + return *last_plan_solution_; } // Clone current planning scene @@ -165,7 +167,8 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) if (current_goal_constraints_.empty()) { ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); - return false; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); + return *last_plan_solution_; } req.goal_constraints = current_goal_constraints_; @@ -174,17 +177,18 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) if (planning_pipeline_names_.find(parameters.planning_pipeline) == planning_pipeline_names_.end()) { ROS_ERROR_NAMED(LOGNAME, "No planning pipeline available for name '%s'", parameters.planning_pipeline.c_str()); - return false; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; } const planning_pipeline::PlanningPipelinePtr pipeline = moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); pipeline->generatePlan(planning_scene, req, res); + last_plan_solution_->error_code = res.error_code_.val; if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); - return false; + return *last_plan_solution_; } - last_plan_solution_.reset(new PlanSolution()); last_plan_solution_->start_state = req.start_state; last_plan_solution_->trajectory = res.trajectory_; // TODO(henningkayser): Visualize trajectory @@ -199,10 +203,10 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); // } //} - return true; + return *last_plan_solution_; } -bool PlanningComponent::plan() +PlanningComponent::PlanSolution PlanningComponent::plan() { PlanRequestParameters default_parameters; default_parameters.planning_attempts = 1; @@ -270,7 +274,7 @@ bool PlanningComponent::setGoal(const std::vector& goa bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) { - current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_.get()) }; + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) }; return true; } @@ -296,7 +300,7 @@ bool PlanningComponent::setGoal(const std::string& goal_state_name) return false; } robot_state::RobotState goal_state(moveit_cpp_->getRobotModel()); - goal_state.setToDefaultValues(joint_model_group_.get(), goal_state_name); + goal_state.setToDefaultValues(joint_model_group_, goal_state_name); return setGoal(goal_state); } @@ -316,7 +320,12 @@ bool PlanningComponent::execute(bool blocking) // ROS_ERROR("Failed to parameterize trajectory"); // return false; //} - moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); + return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); +} + +const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +{ + return last_plan_solution_; } void PlanningComponent::clearContents() @@ -324,7 +333,6 @@ void PlanningComponent::clearContents() considered_start_state_.reset(); last_plan_solution_.reset(); current_goal_constraints_.clear(); - joint_model_group_.reset(); moveit_cpp_.reset(); planning_pipeline_names_.clear(); } diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 979b4e1f92..7228694591 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -5,6 +5,8 @@ if (CATKIN_ENABLE_TESTING) add_executable(test_cleanup test_cleanup.cpp) target_link_libraries(test_cleanup moveit_move_group_interface) + add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) add_rostest(python_move_group.test) add_rostest(python_move_group_ns.test) add_rostest(robot_state_update.test) diff --git a/moveit_ros/planning_interface/test/moveit_cpp.yaml b/moveit_ros/planning_interface/test/moveit_cpp.yaml new file mode 100644 index 0000000000..910b85c700 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp.yaml @@ -0,0 +1,17 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/planning_scene_monitor" + publish_planning_scene_topic: "/publish_planning_scene" + monitored_planning_scene_topic: "/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: + - ompl + +default_planner_options: + planning_attempts: 1 + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 \ No newline at end of file diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp new file mode 100644 index 0000000000..0b956ac074 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -0,0 +1,178 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Abdi + Desc: Test the MoveItCpp and PlanningComponent interfaces +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include +// Msgs +#include + +namespace moveit +{ +namespace planning_interface +{ +class MoveItCppTest : public ::testing::Test +{ +public: + void SetUp() override + { + nh_ = ros::NodeHandle("/moveit_cpp_test"); + moveit_cpp_ptr = std::make_shared(nh_); + planning_component_ptr = std::make_shared(PLANNING_GROUP, moveit_cpp_ptr); + robot_model_ptr = moveit_cpp_ptr->getRobotModel(); + jmg_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP); + + target_pose1.header.frame_id = "panda_link0"; + target_pose1.pose.orientation.w = 1.0; + target_pose1.pose.position.x = 0.28; + target_pose1.pose.position.y = -0.2; + target_pose1.pose.position.z = 0.5; + + start_pose.orientation.w = 1.0; + start_pose.position.x = 0.55; + start_pose.position.y = 0.0; + start_pose.position.z = 0.6; + + target_pose2.orientation.w = 1.0; + target_pose2.position.x = 0.55; + target_pose2.position.y = -0.05; + target_pose2.position.z = 0.8; + } + +protected: + ros::NodeHandle nh_; + MoveitCppPtr moveit_cpp_ptr; + PlanningComponentPtr planning_component_ptr; + robot_model::RobotModelConstPtr robot_model_ptr; + const moveit::core::JointModelGroup* jmg_ptr; + const std::string PLANNING_GROUP = "panda_arm"; + geometry_msgs::PoseStamped target_pose1; + geometry_msgs::Pose target_pose2; + geometry_msgs::Pose start_pose; +}; + +// Test the current and the initial state of the Panda robot +TEST_F(MoveItCppTest, GetCurrentStateTest) +{ + ros::Duration(1.0).sleep(); // Otherwise joint_states will result in an invalid robot state + auto robot_model = moveit_cpp_ptr->getRobotModel(); + auto robot_state = std::make_shared(robot_model); + EXPECT_TRUE(moveit_cpp_ptr->getCurrentState(robot_state, 0.0)); + // Make sure the Panda robot is in "ready" state which is loaded from fake_controller.yaml + std::vector joints_vals; + robot_state->copyJointGroupPositions(PLANNING_GROUP, joints_vals); + EXPECT_NEAR(joints_vals[0], 0.0, 0.001); // panda_joint1 + EXPECT_NEAR(joints_vals[1], -0.785, 0.001); // panda_joint2 + EXPECT_NEAR(joints_vals[2], 0.0, 0.001); // panda_joint3 + EXPECT_NEAR(joints_vals[3], -2.356, 0.001); // panda_joint4 + EXPECT_NEAR(joints_vals[4], 0.0, 0.001); // panda_joint5 + EXPECT_NEAR(joints_vals[5], 1.571, 0.001); // panda_joint6 + EXPECT_NEAR(joints_vals[6], 0.785, 0.001); // panda_joint7 +} + +// Test the name of the planning group used by PlanningComponent for the Panda robot +TEST_F(MoveItCppTest, NameOfPlanningGroupTest) +{ + EXPECT_STREQ(planning_component_ptr->getName().c_str(), "panda_arm"); +} + +// Test setting the start state of the plan to the current state of the robot +TEST_F(MoveItCppTest, TestSetStartStateToCurrentState) +{ + planning_component_ptr->setStartStateToCurrentState(); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); + // TODO(JafarAbdi) adding testing to the soln state +} + +// Test setting the goal using geometry_msgs::PoseStamped and a robot's link name +TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) +{ + planning_component_ptr->setStartStateToCurrentState(); + + geometry_msgs::PoseStamped target_pose1; + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); +} + +// Test setting the plan start state using robot_state::RobotState +TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) +{ + auto start_state = *(moveit_cpp_ptr->getCurrentState()); + start_state.setFromIK(jmg_ptr, start_pose); + + planning_component_ptr->setStartState(start_state); + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); +} + +// Test settting the goal of the plan using a robot_state::RobotState +TEST_F(MoveItCppTest, TestSetGoalFromRobotState) +{ + auto target_state = *(moveit_cpp_ptr->getCurrentState()); + + target_state.setFromIK(jmg_ptr, target_pose2); + + planning_component_ptr->setGoal(target_state); + + ASSERT_TRUE(planning_component_ptr->plan()); +} +} // namespace planning_interface +} // namespace moveit + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "moveit_cpp_test"); + + ros::AsyncSpinner spinner(4); + spinner.start(); + + int result = RUN_ALL_TESTS(); + + return result; +} diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test new file mode 100644 index 0000000000..265fb337dc --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + ["/moveit_cpp_test/fake_controller_joint_states"] + + + + + + + +