diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp index 0ff378a533..7b38375301 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp @@ -568,6 +568,7 @@ void JogCalcs::enforceJointVelocityLimits(Eigen::VectorXd& calculated_joint_chan bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& new_jt_traj) { bool halting = false; + for (auto joint : joint_model_group_->getJointModels()) { if (!kinematic_state_->satisfiesVelocityBounds(joint)) @@ -575,8 +576,9 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() << " " << " close to a " " velocity limit. Enforcing limit."); + kinematic_state_->enforceVelocityBounds(joint); - for (std::size_t c = 0; c < num_joints_; ++c) + for (std::size_t c = 0; c < new_jt_traj.points[0].velocities.size(); ++c) { if (new_jt_traj.joint_names[c] == joint->getName()) { @@ -588,7 +590,7 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n // Halt if we're past a joint margin and joint velocity is moving even farther past double joint_angle = 0; - for (std::size_t c = 0; c < num_joints_; ++c) + for (std::size_t c = 0; c < original_jt_state_.name.size(); ++c) { if (original_jt_state_.name[c] == joint->getName()) { @@ -596,7 +598,6 @@ bool JogCalcs::checkIfJointsWithinURDFBounds(trajectory_msgs::JointTrajectory& n break; } } - if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) { const std::vector limits = joint->getVariableBoundsMsg(); diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index d1b5594b18..d8073f37b2 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -51,6 +51,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS common_planning_interface_objects/include planning_scene_interface/include move_group_interface/include + moveit_cpp/include ) catkin_python_setup() @@ -60,6 +61,7 @@ catkin_package( moveit_common_planning_interface_objects moveit_planning_scene_interface moveit_move_group_interface + moveit_cpp INCLUDE_DIRS ${THIS_PACKAGE_INCLUDE_DIRS} CATKIN_DEPENDS @@ -89,3 +91,4 @@ add_subdirectory(planning_scene_interface) add_subdirectory(move_group_interface) add_subdirectory(robot_interface) add_subdirectory(test) +add_subdirectory(moveit_cpp) diff --git a/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt new file mode 100644 index 0000000000..098becde4b --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/CMakeLists.txt @@ -0,0 +1,12 @@ +set(MOVEIT_LIB_NAME moveit_cpp) + +add_library(${MOVEIT_LIB_NAME} src/moveit_cpp.cpp src/planning_component.cpp) +set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) + +install(TARGETS ${MOVEIT_LIB_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 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 new file mode 100644 index 0000000000..fd81721ec5 --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h @@ -0,0 +1,203 @@ +/********************************************************************* + * 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: Henning Kayser */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(MoveItCpp); + +class MoveItCpp +{ +public: + /// Specification of options to use when constructing the MoveItCpp class + struct PlanningSceneMonitorOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "planning_scene_monitor_options/"; + nh.param(ns + "name", name, "planning_scene_monitor"); + nh.param(ns + "robot_description", robot_description, "robot_description"); + nh.param(ns + "joint_state_topic", joint_state_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC); + nh.param(ns + "attached_collision_object_topic", attached_collision_object_topic, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC); + nh.param(ns + "monitored_planning_scene_topic", monitored_planning_scene_topic, + 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 + { + void load(const ros::NodeHandle& nh) + { + 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; + }; + struct PlannerOptions + { + void load(const ros::NodeHandle& nh) + { + std::string ns = "default_planner_options/"; + 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; + double max_acceleration_scaling_factor; + }; + + /// Parameter container for initializing MoveItCpp + struct Options + { + Options(const ros::NodeHandle& nh) + { + planning_scene_monitor_options.load(nh); + default_planner_options.load(nh); + planning_pipeline_options.load(nh); + } + + PlanningSceneMonitorOptions planning_scene_monitor_options; + PlanningPipelineOptions planning_pipeline_options; + PlannerOptions default_planner_options; + }; + + /** \brief Constructor */ + MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer = {}); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + MoveItCpp(const MoveItCpp&) = delete; + MoveItCpp& operator=(const MoveItCpp&) = delete; + + MoveItCpp(MoveItCpp&& other); + MoveItCpp& operator=(MoveItCpp&& other); + + /** \brief Destructor */ + ~MoveItCpp(); + + /** \brief Get the RobotModel object. */ + robot_model::RobotModelConstPtr getRobotModel() const; + + /** \brief Get the ROS node handle of this instance operates on */ + const ros::NodeHandle& getNodeHandle() const; + + /** \brief Get the current state queried from the current state monitor */ + bool getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds); + + /** \brief Get the current state queried from the current state monitor */ + robot_state::RobotStatePtr getCurrentState(double wait_seconds = 0.0); + + /** \brief Get all loaded planning pipeline instances mapped to their reference names */ + const std::map& getPlanningPipelines() const; + + /** \brief Get the names of all loaded planning pipelines. Specify group_name to filter the results by planning group + */ + std::set getPlanningPipelineNames(const std::string& group_name = "") const; + + /** \brief Get the stored instance of the planning scene monitor */ + const planning_scene_monitor::PlanningSceneMonitorPtr& getPlanningSceneMonitor() const; + planning_scene_monitor::PlanningSceneMonitorPtr getPlanningSceneMonitorNonConst(); + + const std::shared_ptr& getTFBuffer() const; + + /** \brief Get the stored instance of the trajectory execution manager */ + const trajectory_execution_manager::TrajectoryExecutionManagerPtr& getTrajectoryExecutionManager() const; + trajectory_execution_manager::TrajectoryExecutionManagerPtr getTrajectoryExecutionManagerNonConst(); + + /** \brief Execute a trajectory on the planning group specified by group_name using the trajectory execution manager. + * If blocking is set to false, the execution is run in background and the function returns immediately. */ + bool execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking = true); + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + +private: + // Core properties and instances + ros::NodeHandle node_handle_; + std::string robot_description_; + robot_model::RobotModelConstPtr robot_model_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Planning + std::map planning_pipelines_; + std::map> groups_pipelines_map_; + std::map> groups_algorithms_map_; + + // Execution + trajectory_execution_manager::TrajectoryExecutionManagerPtr trajectory_execution_manager_; + + /** \brief Reset all member variables */ + void clearContents(); + + /** \brief Initialize and setup the planning scene monitor */ + bool loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt); + + /** \brief Initialize and setup the planning pipelines */ + bool loadPlanningPipelines(const PlanningPipelineOptions& opt); +}; +} // namespace planning_interface +} // namespace moveit 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 new file mode 100644 index 0000000000..06d8124edf --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/include/moveit/moveit_cpp/planning_component.h @@ -0,0 +1,216 @@ +/********************************************************************* + * 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: Henning Kayser */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +MOVEIT_CLASS_FORWARD(PlanningComponent); + +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 + { + /// The full starting state used for planning + moveit_msgs::RobotState start_state; + + /// 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 + struct PlanRequestParameters + { + std::string planner_id; + std::string planning_pipeline; + size_t planning_attempts; + double planning_time; + double max_velocity_scaling_factor; + double max_acceleration_scaling_factor; + }; + + /** \brief Constructor */ + PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh); + PlanningComponent(const std::string& group_name, const MoveItCppPtr& moveit_context); + + /** + * @brief This class owns unique resources (e.g. action clients, threads) and its not very + * meaningful to copy. Pass by references, move it, or simply create multiple instances where + * required. + */ + PlanningComponent(const PlanningComponent&) = delete; + PlanningComponent& operator=(const PlanningComponent&) = delete; + + PlanningComponent(PlanningComponent&& other); + PlanningComponent& operator=(PlanningComponent&& other); + + /** \brief Destructor */ + ~PlanningComponent(); + + /** \brief Get the name of the planning group */ + const std::string& getName() const; + + /** \brief Get the names of the named robot states available as targets */ + const std::vector getNamedTargets(); + + /** \brief Get the joint values for targets specified by name */ + std::map getNamedTargetValues(const std::string& name); + + /** \brief Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). + This is useful when the planning group contains the root joint of the robot -- i.e. when planning motion for the + robot relative to the world. */ + void setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz); + + /** \brief Remove the workspace bounding box from planning */ + void unsetWorkspace(); + + /** \brief Get the considered start state if defined, otherwise get the current state */ + robot_state::RobotStatePtr getStartState(); + + /** \brief Set the robot state that should be considered as start state for planning */ + bool setStartState(const robot_state::RobotState& start_state); + /** \brief Set the named robot state that should be considered as start state for planning */ + bool setStartState(const std::string& named_state); + + /** \brief Set the start state for planning to be the current state of the robot */ + void setStartStateToCurrentState(); + + /** \brief Set the goal constraints used for planning */ + bool setGoal(const std::vector& goal_constraints); + /** \brief Set the goal constraints generated from a target state */ + bool setGoal(const robot_state::RobotState& goal_state); + /** \brief Set the goal constraints generated from target pose and robot link */ + bool setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name); + /** \brief Set the goal constraints generated from a named target state */ + bool setGoal(const std::string& named_target); + + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using + * default parameters. */ + PlanSolution plan(); + /** \brief Run a plan from start or current state to fulfill the last goal constraints provided by setGoal() using the + * provided PlanRequestParameters. */ + 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_; + // 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_; + robot_state::RobotStatePtr considered_start_state_; + std::vector current_goal_constraints_; + PlanRequestParameters plan_request_parameters_; + moveit_msgs::WorkspaceParameters workspace_parameters_; + bool workspace_parameters_set_ = false; + PlanSolutionPtr last_plan_solution_; + + // common properties for goals + // TODO(henningkayser): support goal tolerances + // double goal_joint_tolerance_; + // double goal_position_tolerance_; + // double goal_orientation_tolerance_; + // TODO(henningkayser): implment path/trajectory constraints + // std::unique_ptr path_constraints_; + // std::unique_ptr trajectory_constraints_; + + /** \brief Reset all member variables */ + void clearContents(); +}; +} // namespace planning_interface +} // namespace moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp new file mode 100644 index 0000000000..f27efed06b --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/moveit_cpp.cpp @@ -0,0 +1,333 @@ +/********************************************************************* + * 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: Henning Kayser */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +constexpr char LOGNAME[] = "moveit_cpp"; +constexpr char PLANNING_SCENE_MONITOR_NAME[] = "moveit_cpp_planning_scene"; +constexpr char PLANNING_PLUGIN_PARAM[] = "planning_plugin"; + +MoveItCpp::MoveItCpp(const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) + : MoveItCpp(Options(nh), nh, tf_buffer) +{ +} + +MoveItCpp::MoveItCpp(const Options& opt, const ros::NodeHandle& nh, const std::shared_ptr& tf_buffer) +{ + if (!tf_buffer_) + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + + // Configure planning scene monitor + if (!loadPlanningSceneMonitor(opt.planning_scene_monitor_options)) + { + std::string error = "Unable to configure planning scene monitor"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + robot_model_ = planning_scene_monitor_->getRobotModel(); + if (!robot_model_) + { + std::string error = "Unable to construct robot model. Please make sure all needed information is on the " + "parameter server."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + bool load_planning_pipelines = true; + if (load_planning_pipelines && !loadPlanningPipelines(opt.planning_pipeline_options)) + { + std::string error = "Failed to load planning pipelines from parameter server"; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + + // TODO(henningkayser): configure trajectory execution manager + trajectory_execution_manager_.reset(new trajectory_execution_manager::TrajectoryExecutionManager( + robot_model_, planning_scene_monitor_->getStateMonitor())); + + ROS_INFO_NAMED(LOGNAME, "MoveItCpp running"); +} + +MoveItCpp::MoveItCpp(MoveItCpp&& other) +{ + other.clearContents(); +} + +MoveItCpp::~MoveItCpp() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting MoveItCpp"); + clearContents(); +} + +MoveItCpp& MoveItCpp::operator=(MoveItCpp&& other) +{ + if (this != &other) + { + this->robot_description_ = other.robot_description_; + this->node_handle_ = other.node_handle_; + this->tf_buffer_ = other.tf_buffer_; + this->robot_model_ = other.robot_model_; + this->planning_scene_monitor_ = other.planning_scene_monitor_; + other.clearContents(); + } + + return *this; +} + +bool MoveItCpp::loadPlanningSceneMonitor(const PlanningSceneMonitorOptions& opt) +{ + planning_scene_monitor_.reset( + new planning_scene_monitor::PlanningSceneMonitor(opt.robot_description, tf_buffer_, opt.name)); + // Allows us to sycronize to Rviz and also publish collision objects to ourselves + ROS_DEBUG_STREAM_NAMED(LOGNAME, "Configuring Planning Scene Monitor"); + if (planning_scene_monitor_->getPlanningScene()) + { + // Start state and scene monitors + ROS_INFO_NAMED(LOGNAME, "Listening to '%s' for joint states", opt.joint_state_topic.c_str()); + planning_scene_monitor_->startStateMonitor(opt.joint_state_topic, opt.attached_collision_object_topic); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + opt.publish_planning_scene_topic); + planning_scene_monitor_->startSceneMonitor(opt.monitored_planning_scene_topic); + } + else + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); + return false; + } + // TODO(henningkayser): fix and remove lines below + ros::spinOnce(); + ros::Duration(0.5).sleep(); // when at 0.1, i believe sometimes vjoint not properly loaded + + // Wait for complete state to be recieved + if (opt.wait_for_initial_state_timeout > 0.0) + { + return planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), + opt.wait_for_initial_state_timeout); + } + + return true; +} + +bool MoveItCpp::loadPlanningPipelines(const PlanningPipelineOptions& opt) +{ + ros::NodeHandle node_handle(opt.parent_namespace.empty() ? "~" : opt.parent_namespace); + for (const auto& planning_pipeline_name : opt.pipeline_names) + { + if (planning_pipelines_.count(planning_pipeline_name) > 0) + { + ROS_WARN_NAMED(LOGNAME, "Skipping duplicate entry for planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + ROS_INFO_NAMED(LOGNAME, "Loading planning pipeline '%s'", planning_pipeline_name.c_str()); + ros::NodeHandle child_nh(node_handle, planning_pipeline_name); + planning_pipeline::PlanningPipelinePtr pipeline; + pipeline.reset(new planning_pipeline::PlanningPipeline(robot_model_, child_nh, PLANNING_PLUGIN_PARAM)); + + if (!pipeline->getPlannerManager()) + { + ROS_ERROR_NAMED(LOGNAME, "Failed to initialize planning pipeline '%s'.", planning_pipeline_name.c_str()); + continue; + } + planning_pipelines_[planning_pipeline_name] = pipeline; + } + + if (planning_pipelines_.empty()) + { + return false; + ROS_ERROR_NAMED(LOGNAME, "Failed to load any planning pipelines."); + } + + // Retrieve group/pipeline mapping for faster lookup + std::vector group_names = robot_model_->getJointModelGroupNames(); + for (const auto& pipeline_entry : planning_pipelines_) + { + for (const auto& group_name : group_names) + { + groups_pipelines_map_[group_name] = {}; + const auto& pipeline = pipeline_entry.second; + for (const auto& planner_configuration : pipeline->getPlannerManager()->getPlannerConfigurations()) + { + if (planner_configuration.second.group == group_name) + { + groups_pipelines_map_[group_name].insert(pipeline_entry.first); + } + } + } + } + + return true; +} + +robot_model::RobotModelConstPtr MoveItCpp::getRobotModel() const +{ + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getRobotModel()"); + return robot_model_; +} + +const ros::NodeHandle& MoveItCpp::getNodeHandle() const +{ + ROS_DEBUG_NAMED(LOGNAME, "MoveItCpp::getNodeHandle()"); + return node_handle_; +} + +bool MoveItCpp::getCurrentState(robot_state::RobotStatePtr& current_state, double wait_seconds) +{ + if (wait_seconds > 0.0 && + !planning_scene_monitor_->getStateMonitor()->waitForCurrentState(ros::Time::now(), wait_seconds)) + { + ROS_ERROR_NAMED(LOGNAME, "Did not receive robot state"); + return false; + } + { // Lock planning scene + planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor_); + current_state.reset(new moveit::core::RobotState(scene->getCurrentState())); + } // Unlock planning scene + return true; +} + +robot_state::RobotStatePtr MoveItCpp::getCurrentState(double wait) +{ + robot_state::RobotStatePtr current_state; + getCurrentState(current_state, wait); + return current_state; +} + +const std::map& MoveItCpp::getPlanningPipelines() const +{ + return planning_pipelines_; +} + +std::set MoveItCpp::getPlanningPipelineNames(const std::string& group_name) const +{ + std::set result_names; + if (!group_name.empty() && groups_pipelines_map_.count(group_name) == 0) + { + ROS_ERROR_NAMED(LOGNAME, "There are no planning pipelines loaded for group '%s'.", group_name.c_str()); + return result_names; // empty + } + for (const auto& pipeline_entry : planning_pipelines_) + { + const std::string& pipeline_name = pipeline_entry.first; + // If group_name is defined and valid, skip pipelines that don't belong to the planning group + if (!group_name.empty()) + { + const auto& group_pipelines = groups_pipelines_map_.at(group_name); + if (group_pipelines.find(pipeline_name) == group_pipelines.end()) + continue; + } + result_names.insert(pipeline_name); + } + return result_names; +} + +const planning_scene_monitor::PlanningSceneMonitorPtr& MoveItCpp::getPlanningSceneMonitor() const +{ + return planning_scene_monitor_; +} +planning_scene_monitor::PlanningSceneMonitorPtr MoveItCpp::getPlanningSceneMonitorNonConst() +{ + return planning_scene_monitor_; +} + +const trajectory_execution_manager::TrajectoryExecutionManagerPtr& MoveItCpp::getTrajectoryExecutionManager() const +{ + return trajectory_execution_manager_; +} + +trajectory_execution_manager::TrajectoryExecutionManagerPtr MoveItCpp::getTrajectoryExecutionManagerNonConst() +{ + return trajectory_execution_manager_; +} + +bool MoveItCpp::execute(const std::string& group_name, const robot_trajectory::RobotTrajectoryPtr& robot_trajectory, + bool blocking) +{ + if (!robot_trajectory) + { + ROS_ERROR_NAMED(LOGNAME, "Robot trajectory is undefined"); + return false; + } + + // Check if there are controllers that can handle the execution + if (!trajectory_execution_manager_->ensureActiveControllersForGroup(group_name)) + { + ROS_ERROR_NAMED(LOGNAME, "Execution failed! No active controllers configured for group '%s'", group_name.c_str()); + return false; + } + + // Execute trajectory + moveit_msgs::RobotTrajectory robot_trajectory_msg; + robot_trajectory->getRobotTrajectoryMsg(robot_trajectory_msg); + if (blocking) + { + trajectory_execution_manager_->push(robot_trajectory_msg); + trajectory_execution_manager_->execute(); + return trajectory_execution_manager_->waitForExecution(); + } + trajectory_execution_manager_->pushAndExecute(robot_trajectory_msg); + return true; +} + +const std::shared_ptr& MoveItCpp::getTFBuffer() const +{ + return tf_buffer_; +} + +void MoveItCpp::clearContents() +{ + robot_description_.clear(); + tf_buffer_.reset(); + planning_scene_monitor_.reset(); + robot_model_.reset(); + planning_pipelines_.clear(); +} +} // planning_interface +} // moveit diff --git a/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp new file mode 100644 index 0000000000..2ce083b17d --- /dev/null +++ b/moveit_ros/planning_interface/moveit_cpp/src/planning_component.cpp @@ -0,0 +1,354 @@ +/********************************************************************* + * 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: Henning Kayser */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace moveit +{ +namespace planning_interface +{ +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_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); + if (!joint_model_group_) + { + std::string error = "Could not find joint model group '" + group_name + "'."; + ROS_FATAL_STREAM_NAMED(LOGNAME, error); + throw std::runtime_error(error); + } + planning_pipeline_names_ = moveit_cpp_->getPlanningPipelineNames(group_name); +} + +PlanningComponent::PlanningComponent(const std::string& group_name, const ros::NodeHandle& nh) + : group_name_(group_name), nh_(nh), moveit_cpp_(new MoveItCpp(nh)) +{ +} + +PlanningComponent::~PlanningComponent() +{ + ROS_INFO_NAMED(LOGNAME, "Deleting PlanningComponent '%s'", group_name_.c_str()); + clearContents(); +} + +PlanningComponent& PlanningComponent::operator=(PlanningComponent&& other) +{ + if (this != &other) + { + this->considered_start_state_ = other.considered_start_state_; + this->workspace_parameters_ = other.workspace_parameters_; + this->last_plan_solution_ = other.last_plan_solution_; + other.clearContents(); + } + return *this; +} + +const std::vector PlanningComponent::getNamedTargets() +{ + if (joint_model_group_) + { + return joint_model_group_->getDefaultStateNames(); + } + else + { + ROS_WARN_NAMED(LOGNAME, "Unable to find joint group with name '%s'.", group_name_.c_str()); + } + + std::vector empty; + return empty; +} + +const std::string& PlanningComponent::getName() const +{ + return group_name_; +} + +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()); + last_plan_solution_->error_code = MoveItErrorCode(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME); + return *last_plan_solution_; + } + + // Clone current planning scene + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = + moveit_cpp_->getPlanningSceneMonitorNonConst(); + planning_scene_monitor->updateFrameTransforms(); + planning_scene_monitor->lockSceneRead(); // LOCK planning scene + planning_scene::PlanningScenePtr planning_scene = + planning_scene::PlanningScene::clone(planning_scene_monitor->getPlanningScene()); + planning_scene_monitor->unlockSceneRead(); // UNLOCK planning scene + planning_scene_monitor.reset(); // release this pointer + + // Init MotionPlanRequest + ::planning_interface::MotionPlanRequest req; + req.group_name = group_name_; + req.planner_id = parameters.planner_id; + req.allowed_planning_time = parameters.planning_time; + req.max_velocity_scaling_factor = parameters.max_velocity_scaling_factor; + req.max_acceleration_scaling_factor = parameters.max_acceleration_scaling_factor; + if (workspace_parameters_set_) + req.workspace_parameters = workspace_parameters_; + + // Set start state + moveit::core::RobotStatePtr start_state = considered_start_state_; + if (!start_state) + start_state = moveit_cpp_->getCurrentState(); + moveit::core::robotStateToRobotStateMsg(*start_state, req.start_state); + planning_scene->setCurrentState(*start_state); + + // Set goal constraints + 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::INVALID_GOAL_CONSTRAINTS); + return *last_plan_solution_; + } + req.goal_constraints = current_goal_constraints_; + + // Run planning attempt + ::planning_interface::MotionPlanResponse res; + 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()); + 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 *last_plan_solution_; + } + last_plan_solution_->start_state = req.start_state; + last_plan_solution_->trajectory = res.trajectory_; + // TODO(henningkayser): Visualize trajectory + // std::vector eef_links; + // if (joint_model_group->getEndEffectorTips(eef_links)) + //{ + // for (const auto& eef_link : eef_links) + // { + // ROS_INFO_STREAM("Publishing trajectory for end effector " << eef_link->getName()); + // visual_tools_->publishTrajectoryLine(last_solution_trajectory_, eef_link); + // visual_tools_->publishTrajectoryPath(last_solution_trajectory_, false); + // visual_tools_->publishRobotState(last_solution_trajectory_->getLastWayPoint(), rviz_visual_tools::TRANSLUCENT); + // } + //} + return *last_plan_solution_; +} + +PlanningComponent::PlanSolution PlanningComponent::plan() +{ + PlanRequestParameters default_parameters; + default_parameters.planning_attempts = 1; + default_parameters.planning_time = 5.0; + default_parameters.max_velocity_scaling_factor = 1.0; + default_parameters.max_acceleration_scaling_factor = 1.0; + if (!planning_pipeline_names_.empty()) + default_parameters.planning_pipeline = *planning_pipeline_names_.begin(); + return plan(default_parameters); +} + +bool PlanningComponent::setStartState(const robot_state::RobotState& start_state) +{ + considered_start_state_.reset(new robot_state::RobotState(start_state)); + return true; +} + +robot_state::RobotStatePtr PlanningComponent::getStartState() +{ + if (considered_start_state_) + return considered_start_state_; + else + { + robot_state::RobotStatePtr s; + moveit_cpp_->getCurrentState(s, 1.0); + return s; + } +} + +bool PlanningComponent::setStartState(const std::string& start_state_name) +{ + const auto& named_targets = getNamedTargets(); + if (std::find(named_targets.begin(), named_targets.end(), start_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", start_state_name.c_str()); + return false; + } + robot_state::RobotState start_state(moveit_cpp_->getRobotModel()); + start_state.setToDefaultValues(joint_model_group_, start_state_name); + return setStartState(start_state); +} + +void PlanningComponent::setStartStateToCurrentState() +{ + considered_start_state_.reset(); +} + +std::map PlanningComponent::getNamedTargetValues(const std::string& name) +{ + // TODO(henningkayser): verify result + std::map positions; + joint_model_group_->getVariableDefaultPositions(name, positions); + return positions; +} + +void PlanningComponent::setWorkspace(double minx, double miny, double minz, double maxx, double maxy, double maxz) +{ + workspace_parameters_.header.frame_id = moveit_cpp_->getRobotModel()->getModelFrame(); + workspace_parameters_.header.stamp = ros::Time::now(); + workspace_parameters_.min_corner.x = minx; + workspace_parameters_.min_corner.y = miny; + workspace_parameters_.min_corner.z = minz; + workspace_parameters_.max_corner.x = maxx; + workspace_parameters_.max_corner.y = maxy; + workspace_parameters_.max_corner.z = maxz; + workspace_parameters_set_ = true; +} + +void PlanningComponent::unsetWorkspace() +{ + workspace_parameters_set_ = false; +} + +bool PlanningComponent::setGoal(const std::vector& goal_constraints) +{ + current_goal_constraints_ = goal_constraints; + return true; +} + +bool PlanningComponent::setGoal(const robot_state::RobotState& goal_state) +{ + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group_) }; + return true; +} + +bool PlanningComponent::setGoal(const geometry_msgs::PoseStamped& goal_pose, const std::string& link_name) +{ + const auto& joint_names = joint_model_group_->getLinkModelNames(); + // if (std::find(joint_names.begin(), joint_names.end(), link_name) == joint_names.end()) + //{ + // ROS_ERROR_NAMED(LOGNAME, "Link '%s' is not part of joint model group '%s'.", link_name.c_str(), + // group_name_.c_str()); + // return false; + //} + current_goal_constraints_ = { kinematic_constraints::constructGoalConstraints(link_name, goal_pose) }; + return true; +} + +bool PlanningComponent::setGoal(const std::string& goal_state_name) +{ + const auto& named_targets = getNamedTargets(); + if (std::find(named_targets.begin(), named_targets.end(), goal_state_name) == named_targets.end()) + { + ROS_ERROR_NAMED(LOGNAME, "No predefined joint state found for target name '%s'", goal_state_name.c_str()); + return false; + } + robot_state::RobotState goal_state(moveit_cpp_->getRobotModel()); + goal_state.setToDefaultValues(joint_model_group_, goal_state_name); + return setGoal(goal_state); +} + +bool PlanningComponent::execute(bool blocking) +{ + if (!last_plan_solution_) + { + ROS_ERROR_NAMED(LOGNAME, "There is no successfull plan to execute"); + return false; + } + + // TODO(henningkayser): parameterize timestamps if required + // trajectory_processing::TimeOptimalTrajectoryGeneration totg; + // if (!totg.computeTimeStamps(*last_solution_trajectory_, max_velocity_scaling_factor_, + // max_acceleration_scaling_factor_)) + //{ + // ROS_ERROR("Failed to parameterize trajectory"); + // return false; + //} + return moveit_cpp_->execute(group_name_, last_plan_solution_->trajectory, blocking); +} + +const PlanningComponent::PlanSolutionPtr PlanningComponent::getLastPlanSolution() +{ + return last_plan_solution_; +} + +void PlanningComponent::clearContents() +{ + considered_start_state_.reset(); + last_plan_solution_.reset(); + current_goal_constraints_.clear(); + moveit_cpp_.reset(); + planning_pipeline_names_.clear(); +} +} // planning_interface +} // moveit 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..13f17bd09a --- /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"] + + + + + + + +