From 7544d90a38fac69a5fc7fdc011393b017425a15c Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Tue, 10 Sep 2019 13:50:23 +0300 Subject: [PATCH 01/13] First attempt to test moveit_cpp Update launch and Config files Update launch and config files Use raw pointer for joint_model_group_ member variable Use MoveItErrorCodes for plan functions Add wait_for_initial_state_timeout parameter to planning scene monitor options Update moveit_cpp_test.cpp Add wait_for_initial_state_timeout parameter moveit_cpp.yaml --- .../include/moveit/moveit_cpp/moveit_cpp.h | 2 + .../moveit/moveit_cpp/planning_component.h | 39 ++++- .../moveit_cpp/src/moveit.cpp | 8 +- .../moveit_cpp/src/planning_component.cpp | 26 ++-- .../planning_interface/test/CMakeLists.txt | 2 + .../planning_interface/test/moveit_cpp.yaml | 17 +++ .../test/moveit_cpp_test.cpp | 137 ++++++++++++++++++ .../test/moveit_cpp_test.test | 34 +++++ 8 files changed, 246 insertions(+), 19 deletions(-) create mode 100644 moveit_ros/planning_interface/test/moveit_cpp.yaml create mode 100644 moveit_ros/planning_interface/test/moveit_cpp_test.cpp create mode 100644 moveit_ros/planning_interface/test/moveit_cpp_test.test 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..a14699adef 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,6 +68,7 @@ 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; @@ -75,6 +76,7 @@ class MoveitCpp 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 { 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..0a4654c18f 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 { @@ -133,21 +163,24 @@ 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(); + MoveItErrorCode 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); + MoveItErrorCode 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*/ + PlanSolutionPtr getLastPlanSolution(); + private: // Core properties and instances ros::NodeHandle nh_; MoveitCppPtr moveit_cpp_; const std::string group_name_; - moveit::core::JointModelGroupConstPtr joint_model_group_; + 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..62f09bd53a 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,12 @@ const std::string& PlanningComponent::getName() const return group_name_; } -bool PlanningComponent::plan(const PlanRequestParameters& parameters) +PlanningComponent::MoveItErrorCode PlanningComponent::plan(const PlanRequestParameters& parameters) { if (!joint_model_group_) { ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - return false; + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } // Clone current planning scene @@ -165,7 +165,7 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) if (current_goal_constraints_.empty()) { ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); - return false; + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } req.goal_constraints = current_goal_constraints_; @@ -174,7 +174,7 @@ 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; + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } const planning_pipeline::PlanningPipelinePtr pipeline = moveit_cpp_->getPlanningPipelines().at(parameters.planning_pipeline); @@ -182,7 +182,7 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); - return false; + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); } last_plan_solution_.reset(new PlanSolution()); last_plan_solution_->start_state = req.start_state; @@ -199,10 +199,10 @@ bool PlanningComponent::plan(const PlanRequestParameters& parameters) // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); // } //} - return true; + return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); } -bool PlanningComponent::plan() +PlanningComponent::MoveItErrorCode PlanningComponent::plan() { PlanRequestParameters default_parameters; default_parameters.planning_attempts = 1; @@ -270,7 +270,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 +296,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); } @@ -319,12 +319,16 @@ bool PlanningComponent::execute(bool blocking) moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); } +PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +{ + return last_plan_solution_; +} + 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..2573440cc0 --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -0,0 +1,137 @@ +// 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_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)); + // 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 +} + +// TODO(JafarAbdi) unnecessary +TEST_F(MoveItCppTest, NameOfPlanningGroupTest) +{ + EXPECT_STREQ(planning_component_ptr->getName().c_str(), "panda_arm"); +} + +TEST_F(MoveItCppTest, TestSetStartStateToCurrentState) +{ + planning_component_ptr->setStartStateToCurrentState(); + // TODO(JafarAbdi) shouldn't be here v + planning_component_ptr->setGoal(target_pose1, "panda_link8"); + + ASSERT_TRUE(planning_component_ptr->plan()); + // TODO(JafarAbdi) adding testing to the soln state +} + +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_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_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(); + + spinner.stop(); + ros::shutdown(); + 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..07ecc0758a --- /dev/null +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + ["/moveit_cpp_test/fake_controller_joint_states"] + + + + + + + + From 6c8c70fcff3d485e2576e5fa9776464543ee61e7 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 13:13:24 +0300 Subject: [PATCH 02/13] Make Plan function return PlanSolution --- .../moveit/moveit_cpp/planning_component.h | 12 +++++++++-- .../moveit_cpp/src/planning_component.cpp | 21 ++++++++++++------- 2 files changed, 23 insertions(+), 10 deletions(-) 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 0a4654c18f..582dc0370a 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 @@ -92,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 @@ -163,10 +171,10 @@ class PlanningComponent /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using * default parameters. */ - MoveItErrorCode 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. */ - MoveItErrorCode 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. */ 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 62f09bd53a..011c68bf27 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -126,12 +126,14 @@ const std::string& PlanningComponent::getName() const return group_name_; } -PlanningComponent::MoveItErrorCode 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 MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; } // Clone current planning scene @@ -165,7 +167,8 @@ PlanningComponent::MoveItErrorCode PlanningComponent::plan(const PlanRequestPara if (current_goal_constraints_.empty()) { ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; } req.goal_constraints = current_goal_constraints_; @@ -174,7 +177,8 @@ PlanningComponent::MoveItErrorCode PlanningComponent::plan(const PlanRequestPara 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 MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + 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); @@ -182,11 +186,12 @@ PlanningComponent::MoveItErrorCode PlanningComponent::plan(const PlanRequestPara if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + return *last_plan_solution_; } - last_plan_solution_.reset(new PlanSolution()); last_plan_solution_->start_state = req.start_state; last_plan_solution_->trajectory = res.trajectory_; + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); // TODO(henningkayser): Visualize trajectory // std::vector eef_links; // if (joint_model_group->getEndEffectorTips(eef_links)) @@ -199,10 +204,10 @@ PlanningComponent::MoveItErrorCode PlanningComponent::plan(const PlanRequestPara // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); // } //} - return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); + return *last_plan_solution_; } -PlanningComponent::MoveItErrorCode PlanningComponent::plan() +PlanningComponent::PlanSolution PlanningComponent::plan() { PlanRequestParameters default_parameters; default_parameters.planning_attempts = 1; From bb5c83ca77e151bc75d60dafb6ab7fbb847019c6 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 13:14:02 +0300 Subject: [PATCH 03/13] moveit_cpp_test.test: Fix indentation --- moveit_ros/planning_interface/test/moveit_cpp_test.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index 07ecc0758a..476d479e3e 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -1,5 +1,5 @@ - + From 467012c298874aec060939bc243e8fdada7b8f6a Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 13:16:01 +0300 Subject: [PATCH 04/13] Make getLastPlanSolution function return const pointer to PlanSolution --- .../moveit_cpp/include/moveit/moveit_cpp/planning_component.h | 2 +- .../planning_interface/moveit_cpp/src/planning_component.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 582dc0370a..a22010eadc 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 @@ -181,7 +181,7 @@ class PlanningComponent bool execute(bool blocking = true); /** \brief Return the last plan solution*/ - PlanSolutionPtr getLastPlanSolution(); + const PlanSolutionPtr getLastPlanSolution(); private: // Core properties and instances 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 011c68bf27..4ae9d8b0be 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -324,7 +324,7 @@ bool PlanningComponent::execute(bool blocking) moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); } -PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() { return last_plan_solution_; } From 514eca396b0cb51e3620921f6329dfb46e527dfc Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 13:19:18 +0300 Subject: [PATCH 05/13] moveit_cpp_test.test: fix typo --- moveit_ros/planning_interface/test/moveit_cpp_test.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index 476d479e3e..272d3bd86f 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -21,7 +21,7 @@ - + ["/moveit_cpp_test/fake_controller_joint_states"] From 4312b6b6e54054e3238d960cc4a69bbdc35f8b99 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 13:26:18 +0300 Subject: [PATCH 06/13] joint_model_group_: Add a note about freeing the resources --- .../moveit_cpp/include/moveit/moveit_cpp/planning_component.h | 1 + 1 file changed, 1 insertion(+) 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 a22010eadc..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 @@ -188,6 +188,7 @@ class PlanningComponent ros::NodeHandle nh_; MoveitCppPtr moveit_cpp_; const std::string group_name_; + // The robot_model_ member variable of MoveItCpp class will manually free the joint_model_group_ resources const moveit::core::JointModelGroup* joint_model_group_; // Planning From e11eca0c050c8cb3e91710b21b6b2056445cc888 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 13:41:33 +0300 Subject: [PATCH 07/13] moveit_cpp_test.cpp: add description for each test, Clean the tests --- .../test/moveit_cpp_test.cpp | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp index 2573440cc0..bc23276fae 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -54,39 +54,42 @@ class MoveItCppTest : public ::testing::Test 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)); - // 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 + // 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 } -// TODO(JafarAbdi) unnecessary +// 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(); - // TODO(JafarAbdi) shouldn't be here v 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(); @@ -97,6 +100,7 @@ TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped) 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()); @@ -108,6 +112,7 @@ TEST_F(MoveItCppTest, TestSetStartStateFromRobotState) 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()); @@ -131,7 +136,5 @@ int main(int argc, char** argv) int result = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); return result; } From c78330c29f61b625372d1a67f649e637b8d7b789 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 19 Sep 2019 17:37:32 +0300 Subject: [PATCH 08/13] moveit_cpp_test.test: Fix tab indentation --- moveit_ros/planning_interface/test/moveit_cpp_test.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index 272d3bd86f..e572f30a8f 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -1,5 +1,5 @@ - + From 0426e5ac49df41cf78c0e895108f890d3cbae5be Mon Sep 17 00:00:00 2001 From: Jafar Abdi Date: Thu, 19 Sep 2019 17:38:24 +0300 Subject: [PATCH 09/13] Update moveit_cpp_test.test --- moveit_ros/planning_interface/test/moveit_cpp_test.test | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.test b/moveit_ros/planning_interface/test/moveit_cpp_test.test index e572f30a8f..265fb337dc 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.test +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.test @@ -5,7 +5,7 @@ - + From 19440bb7d985f531e3514a04a4d4f364f7129902 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 23 Sep 2019 19:43:29 +0300 Subject: [PATCH 10/13] Addressed Henning review comments --- .../moveit_cpp/src/planning_component.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) 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 4ae9d8b0be..b3f312c40a 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -132,7 +132,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (!joint_model_group_) { ROS_ERROR_NAMED(LOGNAME, "Failed to retrieve joint model group for name '%s'.", group_name_.c_str()); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); return *last_plan_solution_; } @@ -167,7 +167,7 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet if (current_goal_constraints_.empty()) { ROS_ERROR_NAMED(LOGNAME, "No goal constraints set for planning request"); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS); return *last_plan_solution_; } req.goal_constraints = current_goal_constraints_; @@ -183,15 +183,14 @@ PlanningComponent::PlanSolution PlanningComponent::plan(const PlanRequestParamet 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"); - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE); return *last_plan_solution_; } last_plan_solution_->start_state = req.start_state; last_plan_solution_->trajectory = res.trajectory_; - last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::SUCCESS); // TODO(henningkayser): Visualize trajectory // std::vector eef_links; // if (joint_model_group->getEndEffectorTips(eef_links)) From a708ddf7316a0d1de26ad5e61807a6d7c89e16a8 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 23 Sep 2019 19:44:17 +0300 Subject: [PATCH 11/13] Add license --- .../test/moveit_cpp_test.cpp | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp index bc23276fae..0b956ac074 100644 --- a/moveit_ros/planning_interface/test/moveit_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/moveit_cpp_test.cpp @@ -1,3 +1,41 @@ +/********************************************************************* + * 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 From ec34be2761d2d78e8861196eb1cca8dfdfe5f2e3 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 23 Sep 2019 19:50:45 +0300 Subject: [PATCH 12/13] Remove extra semicolons --- .../moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 a14699adef..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 @@ -69,7 +69,7 @@ class MoveitCpp 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; @@ -85,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; }; @@ -97,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; From c45e21525a79d9717024126b6502106b383c36c0 Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Mon, 23 Sep 2019 19:51:06 +0300 Subject: [PATCH 13/13] Add return to execute function --- .../planning_interface/moveit_cpp/src/planning_component.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b3f312c40a..e490f81694 100644 --- a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -320,7 +320,7 @@ 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()