diff --git a/COLCON_IGNORE b/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/moveit_reach_plugins/CMakeLists.txt b/moveit_reach_plugins/CMakeLists.txt index 8792da95..f157a855 100644 --- a/moveit_reach_plugins/CMakeLists.txt +++ b/moveit_reach_plugins/CMakeLists.txt @@ -1,41 +1,24 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(moveit_reach_plugins) add_compile_options(-std=c++11) -find_package(catkin REQUIRED COMPONENTS - eigen_conversions - interactive_markers - moveit_core - moveit_msgs - moveit_ros_planning_interface - pluginlib - reach_core - reach_msgs - visualization_msgs - xmlrpcpp -) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(interactive_markers REQUIRED) +find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(reach_core REQUIRED) +find_package(reach_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometric_shapes REQUIRED) +#find_package(pcl_conversions REQUIRED) +find_package(PCL REQUIRED) -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME}_utils - evaluation_plugins - ik_solver_plugins - reach_display_plugins - CATKIN_DEPENDS - eigen_conversions - interactive_markers - moveit_core - moveit_msgs - moveit_ros_planning_interface - reach_core - reach_msgs - pluginlib - visualization_msgs - xmlrpcpp -) ########### ## BUILD ## @@ -43,19 +26,24 @@ catkin_package( include_directories( include - ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${reach_core_INCLUDE_DIRS} ) # Utils Library add_library(${PROJECT_NAME}_utils src/utils.cpp ) -add_dependencies(${PROJECT_NAME}_utils - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +target_include_directories(${PROJECT_NAME}_utils + PUBLIC + $ + $ ) -target_link_libraries(${PROJECT_NAME}_utils - ${catkin_LIBRARIES} +ament_target_dependencies(${PROJECT_NAME}_utils + geometric_shapes + moveit_msgs + reach_msgs + tf2_eigen ) # Evaluation Plugins @@ -64,53 +52,77 @@ add_library(evaluation_plugins src/evaluation/joint_penalty_moveit.cpp src/evaluation/distance_penalty_moveit.cpp ) -add_dependencies(evaluation_plugins - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +target_include_directories(evaluation_plugins + PUBLIC + $ + $ ) target_link_libraries(evaluation_plugins - ${catkin_LIBRARIES} ${PROJECT_NAME}_utils + ${PCL_LIBRARIES} ) +ament_target_dependencies(evaluation_plugins + geometric_shapes + moveit_msgs + reach_msgs + tf2_eigen + moveit_ros_planning_interface + moveit_core + pluginlib +) +pluginlib_export_plugin_description_file(reach_core eval_plugin_description.xml) # MoveIt IK Solver Plugin add_library(ik_solver_plugins src/ik/moveit_ik_solver.cpp src/ik/discretized_moveit_ik_solver.cpp ) -add_dependencies(ik_solver_plugins - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +target_include_directories(ik_solver_plugins + PUBLIC + $ + $ ) target_link_libraries(ik_solver_plugins - ${catkin_LIBRARIES} ${PROJECT_NAME}_utils ) +ament_target_dependencies(ik_solver_plugins + moveit_ros_planning_interface + geometric_shapes + reach_core + pluginlib + rclcpp + tf2_eigen +) +pluginlib_export_plugin_description_file(reach_core ik_plugin_description.xml) +target_compile_definitions(ik_solver_plugins PRIVATE "MOVEIT_REACH_PLUGINS_BUILDING_LIBRARY") # MoveIt Reach Display Plugin add_library(reach_display_plugins src/display/moveit_reach_display.cpp ) -add_dependencies(reach_display_plugins - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} +target_include_directories(reach_display_plugins + PUBLIC + $ + $ ) target_link_libraries(reach_display_plugins - ${catkin_LIBRARIES} ${PROJECT_NAME}_utils + ${PCL_LIBRARIES} + ik_solver_plugins ) +pluginlib_export_plugin_description_file(reach_core display_plugin_description.xml) # IK Plugin Test -add_executable(ik_plugin_test - test/plugin_test_node.cpp -) -add_dependencies(ik_plugin_test - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) -target_link_libraries(ik_plugin_test - ${catkin_LIBRARIES} -) +#add_executable(ik_plugin_test +# test/plugin_test_node.cpp +#) +#add_dependencies(ik_plugin_test +# ${${PROJECT_NAME}_EXPORTED_TARGETS} +# ${catkin_EXPORTED_TARGETS} +#) +#target_link_libraries(ik_plugin_test +# ${catkin_LIBRARIES} +#) ############# ## INSTALL ## @@ -122,15 +134,24 @@ install( evaluation_plugins ik_solver_plugins reach_display_plugins - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include/ ) -install(FILES plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_export_libraries( + ${PROJECT_NAME}_utils + evaluation_plugins + ik_solver_plugins + reach_display_plugins +) + +ament_export_include_directories( + include ) +ament_package() \ No newline at end of file diff --git a/moveit_reach_plugins/display_plugin_description.xml b/moveit_reach_plugins/display_plugin_description.xml new file mode 100644 index 00000000..468fa0e4 --- /dev/null +++ b/moveit_reach_plugins/display_plugin_description.xml @@ -0,0 +1,9 @@ + + + + + + A reach study display plugin using the MoveIt framework + + + diff --git a/moveit_reach_plugins/plugin_description.xml b/moveit_reach_plugins/eval_plugin_description.xml similarity index 62% rename from moveit_reach_plugins/plugin_description.xml rename to moveit_reach_plugins/eval_plugin_description.xml index 42f66746..fe663ad1 100644 --- a/moveit_reach_plugins/plugin_description.xml +++ b/moveit_reach_plugins/eval_plugin_description.xml @@ -1,5 +1,5 @@ - + @@ -29,30 +29,3 @@ - - - - - - - An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment - - - - - - An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment - This plugin discretizes the target pose around the Z-axis and outputs the solution with the highest score - - - - - - - - - - A reach study display plugin using the MoveIt framework - - - diff --git a/moveit_reach_plugins/ik_plugin_description.xml b/moveit_reach_plugins/ik_plugin_description.xml new file mode 100644 index 00000000..f2942148 --- /dev/null +++ b/moveit_reach_plugins/ik_plugin_description.xml @@ -0,0 +1,16 @@ + + + + + + An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment + + + + + + An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment + This plugin discretizes the target pose around the Z-axis and outputs the solution with the highest score + + + diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/display/moveit_reach_display.h b/moveit_reach_plugins/include/moveit_reach_plugins/display/moveit_reach_display.h index d7ac8d2e..3d3adeb1 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/display/moveit_reach_display.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/display/moveit_reach_display.h @@ -17,16 +17,9 @@ #define MOVEIT_REACH_PLUGINS_MOVEIT_REACH_DISPLAY_H #include +#include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} +#include namespace planning_scene { @@ -36,6 +29,10 @@ typedef std::shared_ptr PlanningScenePtr; namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.MoveItReachDisplay"); + } namespace display { @@ -45,10 +42,12 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase MoveItReachDisplay(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual void showEnvironment() override; + virtual void showEnvironment(const std::vector & names, const std::vector& positions) override; + virtual void updateRobotPose(const std::map& pose) override; private: @@ -59,13 +58,11 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase const moveit::core::JointModelGroup* jmg_; - std::string collision_mesh_filename_; - + std::string collision_mesh_package_; + std::string collision_mesh_filename_path_; std::string collision_mesh_frame_; - ros::NodeHandle nh_; - - ros::Publisher scene_pub_; + rclcpp::Publisher::SharedPtr scene_pub_; }; } // namespace display diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/distance_penalty_moveit.h b/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/distance_penalty_moveit.h index 4749f58f..8baa30b0 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/distance_penalty_moveit.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/distance_penalty_moveit.h @@ -17,17 +17,7 @@ #define MOVEIT_REACH_PLUGINS_EVALUATION_DISTANCE_PENALTY_MOVEIT_H #include -#include - -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} +#include namespace planning_scene { @@ -37,6 +27,10 @@ typedef std::shared_ptr PlanningScenePtr; namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.DistancePenaltyMoveIt"); + } namespace evaluation { @@ -46,7 +40,7 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase DistancePenaltyMoveIt(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node,const std::shared_ptr model) override; virtual double calculateScore(const std::map& pose) override; @@ -62,7 +56,8 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase int exponent_; - std::string collision_mesh_filename_; + std::string collision_mesh_package_; + std::string collision_mesh_filename_path_; std::string collision_mesh_frame_; diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/joint_penalty_moveit.h b/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/joint_penalty_moveit.h index 44a9b7a8..36f76935 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/joint_penalty_moveit.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/joint_penalty_moveit.h @@ -18,18 +18,13 @@ #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.JointPenaltyMoveIt"); + } namespace evaluation { @@ -39,7 +34,7 @@ class JointPenaltyMoveIt : public reach::plugins::EvaluationBase JointPenaltyMoveIt(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual double calculateScore(const std::map& pose) override; diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/manipulability_moveit.h b/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/manipulability_moveit.h index da05b1fe..f58f07b3 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/manipulability_moveit.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/evaluation/manipulability_moveit.h @@ -18,18 +18,12 @@ #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} - namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.ManipulabilityMoveIt"); + } namespace evaluation { @@ -39,7 +33,7 @@ class ManipulabilityMoveIt : public reach::plugins::EvaluationBase ManipulabilityMoveIt(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual double calculateScore(const std::map& pose) override; diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h b/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h index 819941d4..f2473d2a 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/ik/discretized_moveit_ik_solver.h @@ -20,6 +20,7 @@ namespace moveit_reach_plugins { + namespace ik { @@ -29,9 +30,9 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver DiscretizedMoveItIKSolver(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; - virtual boost::optional solveIKFromSeed(const Eigen::Isometry3d& target, + virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map& seed, std::vector& solution) override; diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h b/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h index db419429..f8bca49d 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/ik/moveit_ik_solver.h @@ -18,18 +18,21 @@ #include #include -#include - -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -class RobotState; -} -} +#include + +// PlanningScene +#include + +//namespace moveit +//{ +//namespace core +//{ +//class RobotModel; +//typedef std::shared_ptr RobotModelConstPtr; +//class JointModelGroup; +//class RobotState; +//} +//} namespace planning_scene { @@ -39,6 +42,10 @@ typedef std::shared_ptr PlanningScenePtr; namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.MoveItIKSolver"); + } namespace ik { @@ -48,9 +55,13 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase MoveItIKSolver(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + ~MoveItIKSolver(){ + eval_.reset(); + } + + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; - virtual boost::optional solveIKFromSeed(const Eigen::Isometry3d& target, + virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map &seed, std::vector &solution) override; @@ -63,22 +74,21 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase const double* ik_solution) const; moveit::core::RobotModelConstPtr model_; - planning_scene::PlanningScenePtr scene_; - const moveit::core::JointModelGroup* jmg_; pluginlib::ClassLoader class_loader_; - reach::plugins::EvaluationBasePtr eval_; + // parameters double distance_threshold_; - - std::string collision_mesh_filename_; - + std::string collision_mesh_package_; + std::string collision_mesh_filename_path_; + std::string evaluation_plugin_name_; std::string collision_mesh_frame_; - std::vector touch_links_; + rclcpp::Publisher::SharedPtr scene_pub_; + }; } // namespace ik diff --git a/moveit_reach_plugins/include/moveit_reach_plugins/utils.h b/moveit_reach_plugins/include/moveit_reach_plugins/utils.h index 9e43fb66..e1bb73db 100644 --- a/moveit_reach_plugins/include/moveit_reach_plugins/utils.h +++ b/moveit_reach_plugins/include/moveit_reach_plugins/utils.h @@ -17,11 +17,14 @@ #define MOVEIT_REACH_PLUGINS_KINEMATICS_UTILS_H #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include + +#include namespace moveit_reach_plugins { @@ -35,7 +38,7 @@ namespace moveit_reach_plugins * @param object_name * @return */ - moveit_msgs::CollisionObject createCollisionObject(const std::string &mesh_filename, + moveit_msgs::msg::CollisionObject createCollisionObject(const std::string &mesh_filename, const std::string &parent_link, const std::string &object_name); @@ -46,11 +49,12 @@ namespace moveit_reach_plugins * @param scale * @return */ - visualization_msgs::Marker makeVisual(const reach_msgs::msg::ReachRecord &r, + visualization_msgs::msg::Marker makeVisual(const rclcpp::Node::SharedPtr node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale, const std::string &ns = "reach", - const boost::optional> &color = {}); + const std::optional> &color = {}); /** * @brief makeInteractiveMarker @@ -59,7 +63,8 @@ namespace moveit_reach_plugins * @param scale * @return */ - visualization_msgs::InteractiveMarker makeInteractiveMarker(const reach_msgs::msg::ReachRecord &r, + visualization_msgs::msg::InteractiveMarker makeInteractiveMarker(const rclcpp::Node::SharedPtr node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale); @@ -71,7 +76,8 @@ namespace moveit_reach_plugins * @param ns * @return */ - visualization_msgs::Marker makeMarker(const std::vector &pts, + visualization_msgs::msg::Marker makeMarker(const rclcpp::Node::SharedPtr node, + const std::vector &pts, const std::string &frame, const double scale, const std::string &ns = ""); diff --git a/moveit_reach_plugins/package.xml b/moveit_reach_plugins/package.xml index b794479d..adb692d2 100644 --- a/moveit_reach_plugins/package.xml +++ b/moveit_reach_plugins/package.xml @@ -13,8 +13,8 @@ https://github.com/ros-industrial/reach/issues https://github.com/ros-industrial/reach/ - ament_cmake - eigen_conversions + ament_cmake_ros + tf2_eigen interactive_markers moveit_core moveit_msgs @@ -23,10 +23,9 @@ reach_core reach_msgs visualization_msgs - xmlrpcpp - + ament_cmake diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index f6232b82..cbdba163 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -16,9 +16,12 @@ #include "moveit_reach_plugins/display/moveit_reach_display.h" #include "moveit_reach_plugins/utils.h" #include -#include #include -#include +#include + +// conversions +#include +#include const static std::string PLANNING_SCENE_TOPIC = "planning_scene_display"; @@ -33,99 +36,105 @@ MoveItReachDisplay::MoveItReachDisplay() } -bool MoveItReachDisplay::initialize(XmlRpc::XmlRpcValue& config) +bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!config.hasMember("planning_group") || - !config.hasMember("collision_mesh_filename") || - !config.hasMember("collision_mesh_frame") || - !config.hasMember("fixed_frame") || - !config.hasMember("marker_scale")) + RCLCPP_INFO(LOGGER, "Initializing MoveItReachDisplay!"); + if (!reach::plugins::DisplayBase::initialize(name, node, model)) + { + return false; + } + + std::string param_prefix("display_config."); + std::string planning_group; + + if(!node_->get_parameter(param_prefix + "planning_group", planning_group) || + !node_->get_parameter(param_prefix + "collision_mesh_package", collision_mesh_package_) || + !node_->get_parameter(param_prefix + "collision_mesh_filename_path", collision_mesh_filename_path_) || + !node_->get_parameter(param_prefix + "fixed_frame", fixed_frame_) || + !node_->get_parameter(param_prefix + "collision_mesh_frame", collision_mesh_frame_) || + !node_->get_parameter(param_prefix + "marker_scale", marker_scale_)) { - ROS_ERROR("MoveIt IK Solver Plugin is missing one or more configuration parameters"); + RCLCPP_ERROR(LOGGER, "MoveIt IK Solver Plugin is missing one or more configuration parameters"); return false; } - std::string planning_group; - try - { - planning_group = std::string(config["planning_group"]); - collision_mesh_filename_ = std::string(config["collision_mesh_filename"]); - collision_mesh_frame_ = std::string(config["collision_mesh_frame"]); - fixed_frame_ = std::string(config["fixed_frame"]); - marker_scale_ = double(config["marker_scale"]); - } - catch(const XmlRpc::XmlRpcException& ex) - { - ROS_ERROR_STREAM(ex.getMessage()); - return false; - } +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); - if(!model_) + if(!model_) { - ROS_ERROR("Failed to initialize robot model pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); return false; } jmg_ = model_->getJointModelGroup(planning_group); if(!jmg_) { - ROS_ERROR_STREAM("Failed to get joint model group for '" << planning_group << "'"); + RCLCPP_ERROR_STREAM(LOGGER, "Failed to get joint model group for '" << planning_group << "'"); return false; } - scene_.reset(new planning_scene::PlanningScene (model_)); + scene_ = std::make_shared(model_); // Check that the input collision mesh frame exists if(!scene_->knowsFrameTransform(collision_mesh_frame_)) { - ROS_ERROR_STREAM("Specified collision mesh frame '" << collision_mesh_frame_ << "' does not exist"); + RCLCPP_ERROR_STREAM(LOGGER, "Specified collision mesh frame '" << collision_mesh_frame_ << "' does not exist"); return false; } // Add the collision object to the planning scene const std::string object_name = "reach_object"; - moveit_msgs::CollisionObject obj = utils::createCollisionObject(collision_mesh_filename_, collision_mesh_frame_, object_name); + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(collision_mesh_package_, collision_mesh_frame_, object_name); + if(!scene_->processCollisionObjectMsg(obj)) { - ROS_ERROR("Failed to add collision mesh to planning scene"); + RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); return false; + }else { + RCLCPP_INFO(LOGGER, "Successfully processed collision object '%s'", object_name.c_str()); } - scene_pub_ = nh_.advertise(PLANNING_SCENE_TOPIC, 1, true); + scene_pub_ = node_->create_publisher(PLANNING_SCENE_TOPIC, 1); - ROS_INFO_STREAM("Successfully initialized MoveItReachDisplay plugin"); + RCLCPP_INFO_STREAM(LOGGER, "Successfully initialized MoveItReachDisplay plugin"); return true; } void MoveItReachDisplay::showEnvironment() { - moveit_msgs::PlanningScene scene_msg; + moveit_msgs::msg::PlanningScene scene_msg; scene_->getPlanningSceneMsg(scene_msg); - scene_pub_.publish(scene_msg); + scene_pub_->publish(scene_msg); } void MoveItReachDisplay::updateRobotPose(const std::map& pose) { std::vector joint_names = jmg_->getActiveJointModelNames(); + std::vector joints; if(utils::transcribeInputMap(pose, joint_names, joints)) { - moveit_msgs::PlanningScene scene_msg; + moveit_msgs::msg::PlanningScene scene_msg; scene_msg.is_diff = true; scene_msg.robot_state.is_diff = true; scene_msg.robot_state.joint_state.name = joint_names; scene_msg.robot_state.joint_state.position = joints; - scene_pub_.publish(scene_msg); + scene_pub_->publish(scene_msg); } else { - ROS_ERROR("Failed to transcribe input joints"); + RCLCPP_ERROR(LOGGER, "Failed to transcribe input joints"); } } + void + MoveItReachDisplay::showEnvironment(const std::vector &names, const std::vector &positions) { + + } + } // namespace display } // namespace moveit_reach_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::display::MoveItReachDisplay, reach::plugins::DisplayBase) diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index fc7f875c..41899c60 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -17,7 +17,7 @@ #include "moveit_reach_plugins/utils.h" #include #include -#include +#include namespace moveit_reach_plugins { @@ -30,67 +30,58 @@ DistancePenaltyMoveIt::DistancePenaltyMoveIt() } -bool DistancePenaltyMoveIt::initialize(XmlRpc::XmlRpcValue& config) +bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!config.hasMember("planning_group") || - !config.hasMember("distance_threshold") || - !config.hasMember("collision_mesh_filename") || - !config.hasMember("collision_mesh_frame") || - !config.hasMember("touch_links") || - !config.hasMember("exponent")) + std::string planning_group; + + if(!node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.planning_group", planning_group) || + !node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.distance_threshold", dist_threshold_) || + !node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.collision_mesh_package", collision_mesh_package_) || + !node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.collision_mesh_filename_path", collision_mesh_filename_path_) || + !node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.collision_mesh_frame", collision_mesh_frame_) || + !node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.touch_links", touch_links_) || + !node->get_parameter("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/DistancePenaltyMoveIt.exponent", exponent_)) { - ROS_ERROR("MoveIt Distance Penalty Evaluation plugin is missing one or more configuration parameters"); + RCLCPP_ERROR(LOGGER, "MoveIt Distance Penalty Evaluation plugin is missing one or more configuration parameters"); return false; } - std::string planning_group; - try - { - planning_group = std::string(config["planning_group"]); - dist_threshold_ = double(config["distance_threshold"]); - exponent_ = int(config["exponent"]); - collision_mesh_filename_ = std::string(config["collision_mesh_filename"]); - collision_mesh_frame_ = std::string(config["collision_mesh_frame"]); - for(int i = 0; i < config["touch_links"].size(); ++i) - { - touch_links_.push_back(config["touch_links"][i]); + if (std::find(touch_links_.begin(), touch_links_.end(), "") != touch_links_.end()){ + touch_links_.clear(); } - } - catch(const XmlRpc::XmlRpcException& ex) - { - ROS_ERROR_STREAM(ex.getMessage()); - return false; - } - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; + if(!model_) { - ROS_ERROR("Failed to load robot model"); + RCLCPP_ERROR(LOGGER, "Failed to load robot model"); return false; } jmg_ = model_->getJointModelGroup(planning_group); if(!jmg_) { - ROS_ERROR("Failed to initialize joint model group pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize joint model group pointer"); return false; } - scene_.reset(new planning_scene::PlanningScene (model_)); + scene_ = std::make_shared(model_); // Check that the collision mesh frame exists if(!scene_->knowsFrameTransform(collision_mesh_frame_)) { - ROS_ERROR_STREAM("Specified collision mesh frame '" << collision_mesh_frame_ << "' does not exist"); + RCLCPP_ERROR_STREAM(LOGGER, "Specified collision mesh frame '" << collision_mesh_frame_ << "' does not exist"); return false; } // Add the collision mesh object to the planning scene const std::string object_name = "reach_object"; - moveit_msgs::CollisionObject obj = utils::createCollisionObject(collision_mesh_filename_, collision_mesh_frame_, object_name); + + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(collision_mesh_package_, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { - ROS_ERROR("Failed to add collision mesh to planning scene"); + RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); return false; } else @@ -107,7 +98,7 @@ double DistancePenaltyMoveIt::calculateScore(const std::map std::vector pose_subset; if(!utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames(), pose_subset)) { - ROS_ERROR_STREAM(__FUNCTION__ << ": failed to transcribe input pose map"); + RCLCPP_ERROR_STREAM(LOGGER, __FUNCTION__ << ": failed to transcribe input pose map"); return 0.0f; } @@ -122,5 +113,5 @@ double DistancePenaltyMoveIt::calculateScore(const std::map } // namespace evaluation } // namespace moveit_reach_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::evaluation::DistancePenaltyMoveIt, reach::plugins::EvaluationBase) diff --git a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp index c387f03f..973a1d91 100644 --- a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp @@ -17,7 +17,6 @@ #include "moveit_reach_plugins/utils.h" #include #include -#include namespace moveit_reach_plugins { @@ -30,36 +29,30 @@ JointPenaltyMoveIt::JointPenaltyMoveIt() } -bool JointPenaltyMoveIt::initialize(XmlRpc::XmlRpcValue& config) +bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!config.hasMember("planning_group")) - { - ROS_ERROR("MoveIt Joint Penalty Evaluation Plugin is missing 'planning_group' parameter"); - return false; - } + std::string planning_group; - std::string planning_group; - try + std::string param_prefix("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/JointPenaltyMoveIt."); + if(!node->get_parameter(param_prefix+ "planning_group", planning_group)) { - planning_group = std::string(config["planning_group"]); - } - catch(const XmlRpc::XmlRpcException& ex) - { - ROS_ERROR_STREAM(ex.getMessage()); + RCLCPP_ERROR(LOGGER, "MoveIt Joint Penalty Evaluation Plugin is missing 'planning_group' parameter"); return false; } - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; + if(!model_) { - ROS_ERROR("Failed to initialize robot model pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); return false; } jmg_ = model_->getJointModelGroup(planning_group); if(!jmg_) { - ROS_ERROR("Failed to initialize joint model group pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize joint model group pointer"); return false; } @@ -78,7 +71,7 @@ double JointPenaltyMoveIt::calculateScore(const std::map& p std::vector pose_subset; if(!utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames(), pose_subset)) { - ROS_ERROR_STREAM(__FUNCTION__ << ": failed to transcribe input pose map"); + RCLCPP_ERROR_STREAM(LOGGER, __FUNCTION__ << ": failed to transcribe input pose map"); return 0.0f; } @@ -101,7 +94,7 @@ std::vector> JointPenaltyMoveIt::getJointLimits() const auto& bounds_vec = *limits_vec[i]; if(bounds_vec.size() > 1) { - ROS_FATAL("Joint has more than one DOF; can't pull joint limits correctly"); + RCLCPP_FATAL(LOGGER, "Joint has more than one DOF; can't pull joint limits correctly"); } max.push_back(bounds_vec[0].max_position_); min.push_back(bounds_vec[0].min_position_); @@ -115,5 +108,5 @@ std::vector> JointPenaltyMoveIt::getJointLimits() } // namespace evaluation } // namespace moveit_reach_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::evaluation::JointPenaltyMoveIt, reach::plugins::EvaluationBase) diff --git a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp index 1ab7cd33..53a501fe 100644 --- a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp @@ -17,7 +17,6 @@ #include "moveit_reach_plugins/utils.h" #include #include -#include namespace moveit_reach_plugins { @@ -30,39 +29,32 @@ ManipulabilityMoveIt::ManipulabilityMoveIt() } -bool ManipulabilityMoveIt::initialize(XmlRpc::XmlRpcValue& config) +bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!config.hasMember("planning_group")) - { - ROS_ERROR("MoveIt Manipulability Evaluation Plugin is missing 'planning_group' parameter"); - return false; - } + std::string planning_group; - std::string planning_group; - try + std::string param_prefix ("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/ManipulabilityMoveIt."); + if(!node->get_parameter(param_prefix+ "planning_group", planning_group)) { - planning_group = std::string(config["planning_group"]); - } - catch(const XmlRpc::XmlRpcException& ex) - { - ROS_ERROR_STREAM(ex.getMessage()); + RCLCPP_ERROR(LOGGER, "MoveIt Manipulability Evaluation Plugin is missing 'planning_group' parameter"); return false; } - - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); - if(!model_) +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; + if(!model_) { - ROS_ERROR("Failed to initialize robot model pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); return false; } jmg_ = model_->getJointModelGroup(planning_group); if(!jmg_) { - ROS_ERROR("Failed to initialize joint model group pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize joint model group pointer"); return false; } + RCLCPP_INFO(LOGGER, "moveit_reach_plugins/evaluation/ManipulabilityMoveIt initialized successfully."); return true; } @@ -75,7 +67,7 @@ double ManipulabilityMoveIt::calculateScore(const std::map& std::vector pose_subset; if(!utils::transcribeInputMap(pose, jmg_->getActiveJointModelNames(), pose_subset)) { - ROS_ERROR_STREAM(__FUNCTION__ << ": failed to transcribe input pose map"); + RCLCPP_ERROR_STREAM(LOGGER, __FUNCTION__ << ": failed to transcribe input pose map"); return 0.0f; } @@ -99,5 +91,5 @@ double ManipulabilityMoveIt::calculateScore(const std::map& } // namespace evaluation } // namespace moveit_reach_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::evaluation::ManipulabilityMoveIt, reach::plugins::EvaluationBase) diff --git a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp index ad260700..f7870d4b 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -14,9 +14,6 @@ * limitations under the License. */ #include "moveit_reach_plugins/ik/discretized_moveit_ik_solver.h" -#include -#include -#include #include namespace @@ -43,39 +40,44 @@ DiscretizedMoveItIKSolver::DiscretizedMoveItIKSolver() } -bool DiscretizedMoveItIKSolver::initialize(XmlRpc::XmlRpcValue& config) +bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!MoveItIKSolver::initialize(config)) + if(!MoveItIKSolver::initialize(name, node, model)) { - ROS_ERROR("Failed to initialize MoveItIKSolver plugin"); + RCLCPP_ERROR(LOGGER, "Failed to initialize MoveItIKSolver plugin"); return false; } try { - dt_ = std::abs(double(config["discretization_angle"])); + if(!node->get_parameter("ik_solver_config.discretization_angle", dt_)){ + return false; + } + dt_ = std::abs(double(dt_)); double clamped_dt = clamp(dt_, 0.0, M_PI); if(std::abs(dt_ - clamped_dt) > 1.0e-6) { - ROS_WARN_STREAM("Clamping discretization angle between 0 and pi; new value is " << clamped_dt); + RCLCPP_WARN_STREAM(LOGGER, "Clamping discretization angle between 0 and pi; new value is " << clamped_dt); } dt_ = clamped_dt; } - catch(const XmlRpc::XmlRpcException& ex) + catch(const std::exception& ex) { - ROS_ERROR_STREAM(ex.getMessage()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); return false; } - ROS_INFO_STREAM("Successfully initialized DiscretizedMoveItIKSolver plugin"); + RCLCPP_INFO_STREAM(LOGGER, "Successfully initialized DiscretizedMoveItIKSolver plugin"); return true; } -boost::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& target, +std::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& target, const std::map& seed, std::vector& solution) { - // Calculate the number of discretizations necessary to achieve discretization angle + //RCLCPP_INFO(LOGGER, " TARGET: %f %f %f ", target.translation().x(), target.translation().y(),target.translation().z()); + + // Calculate the number of discretizations necessary to achieve discretization angle const static int n_discretizations = int((2.0*M_PI) / dt_); // Set up containers for the best solution to be saved into the database @@ -86,11 +88,10 @@ boost::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen:: { Eigen::Isometry3d discretized_target (target * Eigen::AngleAxisd (double(i)*dt_, Eigen::Vector3d::UnitZ())); std::vector tmp_solution; - - boost::optional score = MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution); - if(score && (score.get() > best_score)) + std::optional score = MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution); + if(score.has_value() && (score.value() > best_score)) { - best_score = *score; + best_score = score.value(); best_solution = std::move(tmp_solution); } else @@ -102,7 +103,7 @@ boost::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen:: if(best_score > 0) { solution = std::move(best_solution); - return boost::optional(best_score); + return std::optional(best_score); } else { @@ -113,5 +114,5 @@ boost::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen:: } // namespace ik } // namespace moveit_reach_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::ik::DiscretizedMoveItIKSolver, reach::plugins::IKSolverBase) diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 62e9f4ee..3e105d73 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -17,9 +17,7 @@ #include "moveit_reach_plugins/utils.h" #include #include -#include -#include -#include +#include namespace moveit_reach_plugins { @@ -36,82 +34,84 @@ MoveItIKSolver::MoveItIKSolver() } -bool MoveItIKSolver::initialize(XmlRpc::XmlRpcValue& config) +bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!config.hasMember("planning_group") || - !config.hasMember("distance_threshold") || - !config.hasMember("collision_mesh_filename") || - !config.hasMember("collision_mesh_frame") || - !config.hasMember("touch_links") || - !config.hasMember("evaluation_plugin")) + node_ = node; + + std::string planning_group; + + if(!node->get_parameter("ik_solver_config.planning_group", planning_group) || + !node->get_parameter("ik_solver_config.distance_threshold", distance_threshold_) || + !node->get_parameter("ik_solver_config.collision_mesh_package", collision_mesh_package_) || + !node->get_parameter("ik_solver_config.collision_mesh_filename_path", collision_mesh_filename_path_) || + !node->get_parameter("ik_solver_config.collision_mesh_frame", collision_mesh_frame_) || + !node->get_parameter("ik_solver_config.touch_links", touch_links_) || + !node->get_parameter("ik_solver_config.evaluation_plugin.name", evaluation_plugin_name_)) { - ROS_ERROR("MoveIt IK Solver Plugin is missing one or more configuration parameters"); + RCLCPP_ERROR(LOGGER, "MoveIt IK Solver Plugin is missing one or more configuration parameters"); return false; } - std::string planning_group; - try - { - planning_group = std::string(config["planning_group"]); - distance_threshold_ = double(config["distance_threshold"]); - collision_mesh_filename_ = std::string(config["collision_mesh_filename"]); - collision_mesh_frame_ = std::string(config["collision_mesh_frame"]); - - for(int i = 0; i < config["touch_links"].size(); ++i) - { - touch_links_.push_back(config["touch_links"][i]); + if (std::find(touch_links_.begin(), touch_links_.end(), "") != touch_links_.end()){ + touch_links_.clear(); } try { - eval_ = class_loader_.createInstance(config["evaluation_plugin"]["name"]); + eval_ = class_loader_.createSharedInstance(evaluation_plugin_name_); } catch(const pluginlib::ClassLoaderException& ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); } - - if(!eval_->initialize(config["evaluation_plugin"])) + try { - ROS_ERROR_STREAM("Failed to initialize evaluation plugin"); - return false; - } - } - catch(const XmlRpc::XmlRpcException& ex) - { - ROS_ERROR_STREAM(ex.getMessage()); - return false; - } + if(!eval_->initialize(evaluation_plugin_name_, node, model)) + { + RCLCPP_ERROR_STREAM(LOGGER, "Failed to initialize evaluation plugin"); + return false; + } + } + catch(const std::exception& ex) + { + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); + return false; + } + +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); if(!model_) { - ROS_ERROR("Failed to initialize robot model pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); return false; } jmg_ = model_->getJointModelGroup(planning_group); if(!jmg_) { - ROS_ERROR_STREAM("Failed to get joint model group for '" << planning_group << "'"); + RCLCPP_ERROR_STREAM(LOGGER, "Failed to get joint model group for '" << planning_group << "'"); return false; } - scene_.reset(new planning_scene::PlanningScene (model_)); + scene_ = std::make_shared(model_); // Check that the input collision mesh frame exists if(!scene_->knowsFrameTransform(collision_mesh_frame_)) { - ROS_ERROR_STREAM("Specified collision mesh frame '" << collision_mesh_frame_ << "' does not exist"); + RCLCPP_ERROR_STREAM(LOGGER, "Specified collision mesh frame '" << collision_mesh_frame_ << "' does not exist"); return false; } - // Add the collision object to the planning scene + scene_pub_ = node_->create_publisher("ik_planning_scene", 1); + + + // Add the collision object to the planning scene const std::string object_name = "reach_object"; - moveit_msgs::CollisionObject obj = utils::createCollisionObject(collision_mesh_filename_, collision_mesh_frame_, object_name); + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(collision_mesh_package_, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { - ROS_ERROR("Failed to add collision mesh to planning scene"); + RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); return false; } else @@ -119,11 +119,11 @@ bool MoveItIKSolver::initialize(XmlRpc::XmlRpcValue& config) scene_->getAllowedCollisionMatrixNonConst().setEntry(object_name, touch_links_, true); } - ROS_INFO_STREAM("Successfully initialized MoveItIKSolver plugin"); + RCLCPP_INFO_STREAM(LOGGER, "Successfully initialized MoveItIKSolver plugin"); return true; } -boost::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& target, +std::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& target, const std::map& seed, std::vector& solution) { @@ -134,17 +134,21 @@ boost::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& std::vector seed_subset; if(!utils::transcribeInputMap(seed, joint_names, seed_subset)) { - ROS_ERROR_STREAM(__FUNCTION__ << ": failed to transcribe input pose map"); + RCLCPP_ERROR_STREAM(LOGGER, __FUNCTION__ << ": failed to transcribe input pose map"); return {}; } state.setJointGroupPositions(jmg_, seed_subset); state.update(); - const static int SOLUTION_ATTEMPTS = 3; +// const static int SOLUTION_ATTEMPTS = 3; const static double SOLUTION_TIMEOUT = 0.2; - if(state.setFromIK(jmg_, target, SOLUTION_ATTEMPTS, SOLUTION_TIMEOUT, boost::bind(&MoveItIKSolver::isIKSolutionValid, this, _1, _2, _3))) + if(state.setFromIK(jmg_, target, SOLUTION_TIMEOUT, std::bind(&MoveItIKSolver::isIKSolutionValid, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3))) { solution.clear(); state.copyJointGroupPositions(jmg_, solution); @@ -174,6 +178,12 @@ bool MoveItIKSolver::isIKSolutionValid(moveit::core::RobotState* state, const bool colliding = scene_->isStateColliding(*state, jmg->getName(), false); const bool too_close = (scene_->distanceToCollision(*state, scene_->getAllowedCollisionMatrix()) < distance_threshold_); +// if (!colliding && !too_close){ +// scene_->setCurrentState(*state); +// moveit_msgs::msg::PlanningScene scene_msg; +// scene_->getPlanningSceneMsg(scene_msg); +// scene_pub_->publish(scene_msg); +// } return (!colliding && !too_close); } @@ -185,5 +195,5 @@ std::vector MoveItIKSolver::getJointNames() const } // namespace ik } // namespace moveit_reach_plugins -#include +#include PLUGINLIB_EXPORT_CLASS(moveit_reach_plugins::ik::MoveItIKSolver, reach::plugins::IKSolverBase) diff --git a/moveit_reach_plugins/src/utils.cpp b/moveit_reach_plugins/src/utils.cpp index bfa1319d..da33c0e2 100644 --- a/moveit_reach_plugins/src/utils.cpp +++ b/moveit_reach_plugins/src/utils.cpp @@ -18,59 +18,72 @@ #include #include #include -#include -#include + +#include const static double ARROW_SCALE_RATIO = 6.0; const static double NEIGHBOR_MARKER_SCALE_RATIO = ARROW_SCALE_RATIO / 2.0; namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.utils"); + } + namespace utils { - moveit_msgs::CollisionObject createCollisionObject(const std::string &mesh_filename, + moveit_msgs::msg::CollisionObject createCollisionObject(const std::string &mesh_filename, const std::string &parent_link, const std::string &object_name) { // Create a CollisionObject message for the reach object - moveit_msgs::CollisionObject obj; + RCLCPP_INFO(LOGGER, "Creating collision object with mesh_filename: '%s', parent_link: '%s', object_name: '%s'", + mesh_filename.c_str(), parent_link.c_str(), object_name.c_str()); + + moveit_msgs::msg::CollisionObject obj; obj.header.frame_id = parent_link; obj.id = object_name; shapes::ShapeMsg shape_msg; shapes::Mesh *mesh = shapes::createMeshFromResource(mesh_filename); + if (!mesh){ + RCLCPP_ERROR(LOGGER, "Creating Mesh From Resource failed..."); + } shapes::constructMsgFromShape(mesh, shape_msg); - obj.meshes.push_back(boost::get(shape_msg)); + obj.meshes.push_back(boost::get(shape_msg)); obj.operation = obj.ADD; // Assign a default pose to the mesh - geometry_msgs::Pose pose; + geometry_msgs::msg::Pose pose; pose.position.x = pose.position.y = pose.position.z = 0.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0.0; pose.orientation.w = 1.0; obj.mesh_poses.push_back(pose); + return obj; } - visualization_msgs::Marker makeVisual(const reach_msgs::msg::ReachRecord &r, + visualization_msgs::msg::Marker makeVisual(const rclcpp::Node::SharedPtr node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale, const std::string &ns, - const boost::optional> &color) + const std::optional> &color) { static int idx = 0; - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = frame; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = node->now(); marker.ns = ns; marker.id = idx++; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; Eigen::Isometry3d goal_eigen; - tf::poseMsgToEigen(r.goal, goal_eigen); + tf2::fromMsg(r.goal, goal_eigen); // Transform arrow such that arrow x-axis points along goal pose z-axis (Rviz convention) // convert msg parameter goal to Eigen matrix @@ -81,8 +94,7 @@ namespace moveit_reach_plugins goal_eigen = goal_eigen * rot_flip_normal * rot_x_to_z; // Convert back to geometry_msgs pose - geometry_msgs::Pose msg; - tf::poseEigenToMsg(goal_eigen, msg); + geometry_msgs::msg::Pose msg = tf2::toMsg(goal_eigen); marker.pose = msg; marker.scale.x = scale; @@ -118,21 +130,22 @@ namespace moveit_reach_plugins return marker; } - visualization_msgs::InteractiveMarker makeInteractiveMarker(const reach_msgs::msg::ReachRecord &r, + visualization_msgs::msg::InteractiveMarker makeInteractiveMarker(const rclcpp::Node::SharedPtr node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale) { - visualization_msgs::InteractiveMarker m; + visualization_msgs::msg::InteractiveMarker m; m.header.frame_id = frame; m.name = r.id; // Control - visualization_msgs::InteractiveMarkerControl control; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON; + visualization_msgs::msg::InteractiveMarkerControl control; + control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::BUTTON; control.always_visible = true; // Visuals - auto visual = makeVisual(r, frame, scale); + auto visual = makeVisual(node, r, frame, scale); control.markers.push_back(visual); m.controls.push_back(control); @@ -142,17 +155,18 @@ namespace moveit_reach_plugins return m; } - visualization_msgs::Marker makeMarker(const std::vector &pts, + visualization_msgs::msg::Marker makeMarker(const rclcpp::Node::SharedPtr node, + const std::vector &pts, const std::string &frame, const double scale, const std::string &ns) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = frame; - marker.header.stamp = ros::Time::now(); + marker.header.stamp = node->now(); marker.ns = ns; - marker.type = visualization_msgs::Marker::POINTS; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; marker.scale.x = marker.scale.y = marker.scale.z = scale / NEIGHBOR_MARKER_SCALE_RATIO; @@ -175,7 +189,7 @@ namespace moveit_reach_plugins { if (joint_names.size() > input.size()) { - ROS_ERROR("Seed pose size was not at least as large as the number of joints in the planning group"); + RCLCPP_ERROR(LOGGER, "Seed pose size was not at least as large as the number of joints in the planning group"); return false; } @@ -187,7 +201,7 @@ namespace moveit_reach_plugins const auto it = input.find(name); if (it == input.end()) { - ROS_ERROR_STREAM("Joint '" << name << "' in the planning group was not in the input map"); + RCLCPP_ERROR_STREAM(LOGGER, "Joint '" << name << "' in the planning group was not in the input map"); return false; } else diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index c38c7868..80fc95b1 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -4,11 +4,13 @@ project(reach_core) add_compile_options(-std=c++14) find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) find_package(geometry_msgs REQUIRED) find_package(interactive_markers REQUIRED) find_package(moveit_core REQUIRED) -# find_package(PCL REQUIRED) -find_package(pcl_ros REQUIRED) +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) +#find_package(pcl_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -16,6 +18,8 @@ find_package(reach_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) + find_package(OpenMP) if(OPENMP_FOUND) @@ -28,58 +32,74 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs interactive_markers moveit_core - # pcl_ros +# pcl_ros pcl_conversions - pluginlib +# pluginlib rclcpp reach_msgs tf2_eigen tf2_ros visualization_msgs + moveit_ros_planning_interface ) -ament_export_include_directories(include) - -ament_export_libraries( - src/${PROJECT_NAME} - src/${PROJECT_NAME}_plugins -) - -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -# catkin_package( -# INCLUDE_DIRS -# include -# LIBRARIES -# ${PROJECT_NAME} -# ${PROJECT_NAME}_plugins -# CATKIN_DEPENDS -# eigen_conversions -# geometry_msgs -# interactive_markers -# pcl_ros -# pluginlib -# reach_msgs -# tf2_ros -# tf2_eigen -# visualization_msgs -# ) - ########### ## BUILD ## ########### include_directories( include + ${PCL_INCLUDE_DIRS} + ${moveit_ros_planning_interface_INCLUDE_DIRS} ) -include_directories(${PCL_INCLUDE_DIRS}) -# Reach Study Library -add_library(${PROJECT_NAME} +# Plugins Library +add_library(${PROJECT_NAME}_plugins + src/plugins/impl/multiplicative_factory.cpp +) +target_link_libraries(${PROJECT_NAME}_plugins + ${PCL_LIBRARIES} + ${rclcpp_LIBRARIES} + Eigen3::Eigen + ${PROJECT_NAME}_utils +) +target_include_directories(${PROJECT_NAME}_plugins + PUBLIC + $ + $ +) +ament_target_dependencies(${PROJECT_NAME}_plugins + pluginlib + rclcpp + interactive_markers + visualization_msgs + reach_msgs +) +pluginlib_export_plugin_description_file(reach_core plugin_description.xml) + +add_library(${PROJECT_NAME}_utils # Utilities src/utils/general_utils.cpp src/utils/visualization_utils.cpp +) +target_include_directories(${PROJECT_NAME}_utils + PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME}_utils + ${PCL_LIBRARIES} + ${rclcpp_LIBRARIES} +) +ament_target_dependencies(${PROJECT_NAME}_utils + reach_msgs + rclcpp + visualization_msgs +) + + +# Reach Study Library +add_library(${PROJECT_NAME} # Tools src/core/reach_database.cpp src/core/ik_helper.cpp @@ -87,41 +107,43 @@ add_library(${PROJECT_NAME} # Reach Study src/core/reach_study.cpp ) -# target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_plugins -# ) -ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -# Plugins Library -add_library(${PROJECT_NAME}_plugins - src/plugins/impl/multiplicative_factory.cpp +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils) +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -# target_link_libraries(${PROJECT_NAME}_plugins -# ) -ament_target_dependencies(${PROJECT_NAME}_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Reach Study Node add_executable(robot_reach_study_node src/robot_reach_study_node.cpp ) +target_include_directories(robot_reach_study_node + PUBLIC + $ + $ +) target_link_libraries(robot_reach_study_node ${PROJECT_NAME} -) -ament_target_dependencies(robot_reach_study_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${PROJECT_NAME}_utils +# ${PROJECT_NAME}_plugins li promijenio ) -# Load Point Cloud Server Node +## Load Point Cloud Server Node add_executable(load_point_cloud_server_node src/load_point_cloud_server_node.cpp ) -target_link_libraries(load_point_cloud_server_node ${PROJECT_NAME} +target_link_libraries(load_point_cloud_server_node + ${PROJECT_NAME} ) ament_target_dependencies(load_point_cloud_server_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -# Data Loader Node +## Data Loader Node add_executable(data_loader src/data_loader_node.cpp ) @@ -137,11 +159,11 @@ ament_target_dependencies(data_loader ## TEST ## ########## -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest_gtest(${PROJECT_NAME}_plugin_utest test/plugin.test test/plugin_utest.cpp) - target_link_libraries(${PROJECT_NAME}_plugin_utest ${PROJECT_NAME}) -endif() +#if(CATKIN_ENABLE_TESTING) +# find_package(rostest REQUIRED) +# add_rostest_gtest(${PROJECT_NAME}_plugin_utest test/plugin.test test/plugin_utest.cpp) +# target_link_libraries(${PROJECT_NAME}_plugin_utest ${PROJECT_NAME}) +#endif() ############# ## INSTALL ## @@ -151,6 +173,7 @@ install( TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins + ${PROJECT_NAME}_utils robot_reach_study_node load_point_cloud_server_node data_loader @@ -159,14 +182,34 @@ install( RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION include/${PROJECT_NAME} -) +#install( +# TARGETS +# robot_reach_study_node +# DESTINATION lib/${PROJECT_NAME} +#) -install(DIRECTORY launch config +install( + DIRECTORY launch config rviz DESTINATION share/${PROJECT_NAME} ) -install(FILES plugin_description.xml - DESTINATION share/${PROJECT_NAME} +#install(DIRECTORY include/${PROJECT_NAME} +# DESTINATION include/${PROJECT_NAME} +#) + +install(DIRECTORY include/ + DESTINATION include ) + +## EXPORTS +ament_export_include_directories(include) +ament_export_libraries( + ${PROJECT_NAME} + ${PROJECT_NAME}_plugins + ${PROJECT_NAME}_utils +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +ament_package() diff --git a/reach_core/config/reach_study_config.rviz b/reach_core/config/reach_study_config.rviz deleted file mode 100644 index 70b65099..00000000 --- a/reach_core/config/reach_study_config.rviz +++ /dev/null @@ -1,239 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /MarkerArray1/Namespaces1 - Splitter Ratio: 0.5271919965744019 - Tree Height: 695 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /reach_int_markers/update - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /reach_neighbors - Name: Marker - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray - Enabled: false - Marker Topic: /reach_comparison - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: false - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: /planning_scene_display - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base: - Alpha: 1 - Show Axes: false - Show Trail: false - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_b: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_e: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_l: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_r: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_s: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_t: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link_u: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - reach_object: - Alpha: 1 - Show Axes: false - Show Trail: false - tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - tool0: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: base_link - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 11.848441123962402 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.241093710064888 - Y: 0.3581845760345459 - Z: 1.6464532613754272 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.3263669013977051 - Target Frame: - Value: Orbit (rviz) - Yaw: 2.227778196334839 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 992 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000019e00000342fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000004bb0000028200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000342000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000420000000160000000000000000000000010000010f000002f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002f6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005990000034200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1853 - X: 67 - Y: 27 diff --git a/reach_core/include/reach_core/plugins/evaluation_base.h b/reach_core/include/reach_core/plugins/evaluation_base.h index 9ac96442..aa7f5110 100644 --- a/reach_core/include/reach_core/plugins/evaluation_base.h +++ b/reach_core/include/reach_core/plugins/evaluation_base.h @@ -18,7 +18,11 @@ #include #include -#include + +#include + +#include + namespace reach { @@ -43,7 +47,7 @@ namespace reach * @brief initialize * @param config */ - virtual bool initialize(XmlRpc::XmlRpcValue &config) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) = 0; /** * @brief calculateScore diff --git a/reach_core/include/reach_core/plugins/ik_solver_base.h b/reach_core/include/reach_core/plugins/ik_solver_base.h index 3cd9798d..bdd5a720 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -16,12 +16,14 @@ #ifndef REACH_CORE_PLUGINS_IK_IK_SOLVER_BASE_H #define REACH_CORE_PLUGINS_IK_IK_SOLVER_BASE_H -#include +#include #include #include -#include #include +#include +#include + namespace reach { namespace plugins @@ -46,7 +48,7 @@ namespace reach * @param config * @return */ - virtual bool initialize(XmlRpc::XmlRpcValue &config) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) = 0; /** * @brief solveIKFromSeed attempts to find a valid IK solution for the given target pose starting from the input seed state. @@ -55,7 +57,7 @@ namespace reach * @param solution * @return a boost optional type indicating the success of the IK solution and containing the score of the solution */ - virtual boost::optional solveIKFromSeed(const Eigen::Isometry3d &target, + virtual std::optional solveIKFromSeed(const Eigen::Isometry3d &target, const std::map &seed, std::vector &solution) = 0; @@ -64,6 +66,10 @@ namespace reach * @return */ virtual std::vector getJointNames() const = 0; + + public: + rclcpp::Node::SharedPtr node_; + }; typedef std::shared_ptr IKSolverBasePtr; diff --git a/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h b/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h index 606ae8d5..0842c17d 100644 --- a/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h +++ b/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h @@ -29,7 +29,13 @@ namespace reach public: MultiplicativeFactory(); - virtual bool initialize(XmlRpc::XmlRpcValue &config) override; + ~MultiplicativeFactory(){ + for(auto &ev_pl : eval_plugins_){ + ev_pl.reset(); + } + } + + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, std::shared_ptr model) override; virtual double calculateScore(const std::map &pose) override; diff --git a/reach_core/include/reach_core/plugins/reach_display_base.h b/reach_core/include/reach_core/plugins/reach_display_base.h index 7ba8917b..e73ee066 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -19,14 +19,18 @@ #include #include #include -#include +#include #include "reach_core/utils/visualization_utils.h" #include -#include +// PoseStamped +#include -const static std::string INTERACTIVE_MARKER_TOPIC = "reach_int_markers"; -const static std::string REACH_DIFF_TOPIC = "reach_comparison"; -const static std::string MARKER_TOPIC = "reach_neighbors"; +#include + +constexpr char INTERACTIVE_MARKER_TOPIC[] = "reach_int_markers"; +constexpr char REACH_DIFF_TOPIC[] = "reach_comparison"; +constexpr char MARKER_TOPIC[] = "reach_neighbors"; +constexpr char POSE_TOPIC[] = "reach_pose"; namespace reach { @@ -42,33 +46,41 @@ namespace reach { public: - DisplayBase() - : server_(INTERACTIVE_MARKER_TOPIC, node_) - { - diff_pub_ = node_.create_publisher(REACH_DIFF_TOPIC, 1, true); - marker_pub_ = node_.create_publisher(MARKER_TOPIC, 1, true); - } + DisplayBase() = default; virtual ~DisplayBase() { + server_.reset(); + diff_pub_.reset(); + marker_pub_.reset(); } - virtual bool initialize(XmlRpc::XmlRpcValue &config) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model){ + + node_ = node; + server_ = std::make_shared(INTERACTIVE_MARKER_TOPIC, node); + diff_pub_ = node->create_publisher(REACH_DIFF_TOPIC, 1); + marker_pub_ = node->create_publisher(MARKER_TOPIC, 1); + RCLCPP_INFO(LOGGER, "Initialized DisplayBase plugin!"); + return true; + }; virtual void showEnvironment() = 0; + virtual void showEnvironment(const std::vector & names, const std::vector& positions) = 0; + virtual void updateRobotPose(const std::map &pose) = 0; void addInteractiveMarkerData(const reach_msgs::msg::ReachDatabase &database) { - server_.clear(); + server_->clear(); for (const reach_msgs::msg::ReachRecord &rec : database.records) { - auto marker = utils::makeInteractiveMarker(rec, fixed_frame_, marker_scale_); - server_.insert(std::move(marker)); - menu_handler_.apply(server_, rec.id); + auto marker = utils::makeInteractiveMarker(node_, rec, fixed_frame_, marker_scale_); + server_->insert(std::move(marker)); + menu_handler_.apply(*server_, rec.id); } - server_.applyChanges(); + server_->applyChanges(); } void createMenuFunction(const std::string &menu_entry, @@ -79,12 +91,12 @@ namespace reach void updateInteractiveMarker(const reach_msgs::msg::ReachRecord &rec) { - if (server_.erase(rec.id)) + if (server_->erase(rec.id)) { - auto marker = utils::makeInteractiveMarker(rec, fixed_frame_, marker_scale_); - server_.insert(marker); - menu_handler_.apply(server_, rec.id); - server_.applyChanges(); + auto marker = utils::makeInteractiveMarker(node_, rec, fixed_frame_, marker_scale_); + server_->insert(marker); + menu_handler_.apply(*server_, rec.id); + server_->applyChanges(); } else { @@ -101,7 +113,7 @@ namespace reach for (const std::string &id : ids) { visualization_msgs::msg::InteractiveMarker marker; - if (!server_.get(id, marker)) + if (!server_->get(id, marker)) { RCLCPP_ERROR_STREAM(LOGGER, "Failed to get interactive marker '" << id << "' from server"); return; @@ -112,8 +124,8 @@ namespace reach } // Create points marker, publish it, and move robot to result state for given point - visualization_msgs::msg::Marker pt_marker = utils::makeMarker(pt_array, fixed_frame_, marker_scale_); - marker_pub_.publish(pt_marker); + visualization_msgs::msg::Marker pt_marker = reach::utils::makeMarker(node_, pt_array, fixed_frame_, marker_scale_); + marker_pub_->publish(pt_marker); } } @@ -138,7 +150,7 @@ namespace reach for (char perm_ind = 1; perm_ind < static_cast(n_perm - 1); ++perm_ind) { - std::string ns_name = ""; + std::string ns_name(""); for (auto it = data.begin(); it != data.end(); ++it) { if (((perm_ind >> std::distance(data.begin(), it)) & 1) == 1) @@ -184,31 +196,31 @@ namespace reach if (code != 0 && code != n_perm - 1) { std::string ns = {ns_vec[static_cast(code)]}; - visualization_msgs::msg::Marker arrow_marker = utils::makeVisual(data.begin()->second.records[i], fixed_frame_, marker_scale_, ns, {arrow_color}); + visualization_msgs::msg::Marker arrow_marker = utils::makeVisual(node_, data.begin()->second.records[i], fixed_frame_, marker_scale_, ns, {arrow_color}); marker_array.markers.push_back(arrow_marker); } } - diff_pub_.publish(marker_array); + diff_pub_->publish(marker_array); } + public: + std::shared_ptr node_; protected: - std::string fixed_frame_ = "base_frame"; + std::string fixed_frame_ = "base"; double marker_scale_ = 1.0; private: - interactive_markers::InteractiveMarkerServer server_; + std::shared_ptr server_; interactive_markers::MenuHandler menu_handler_; - std::shared_ptr node_; - std::shared_ptr> diff_pub_; std::shared_ptr> marker_pub_; }; - typedef boost::shared_ptr DisplayBasePtr; + typedef std::shared_ptr DisplayBasePtr; } // namespace plugins } // namespace reach diff --git a/reach_core/include/reach_core/reach_database.h b/reach_core/include/reach_core/reach_database.h index 8f188584..9e5dfd0b 100644 --- a/reach_core/include/reach_core/reach_database.h +++ b/reach_core/include/reach_core/reach_database.h @@ -18,9 +18,10 @@ #include "reach_core/study_parameters.h" #include -#include #include #include +#include +#include namespace reach { @@ -90,7 +91,7 @@ namespace reach * @param id * @return */ - boost::optional get(const std::string &id) const; + std::optional get(const std::string &id) const; /** * @brief put adds a ReachRecord message to the database diff --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index 9a07a1b2..4c5bbb97 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -21,9 +21,25 @@ #include #include #include -// #include #include #include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" + +#include + +// +//namespace moveit +//{ +// namespace core +// { +// class RobotModel; +// typedef std::shared_ptr RobotModelConstPtr; +// class JointModelGroup; +// } +//} namespace reach { @@ -33,14 +49,16 @@ namespace reach /** * @brief The ReachStudy class */ - class ReachStudy - { + class ReachStudy { + public: /** * @brief ReachStudy * @param nh */ - ReachStudy(const rclcpp::Node::SharedPtr &node); + ReachStudy(const rclcpp::Node::SharedPtr node); + + ~ReachStudy(); /** * @brief run @@ -49,10 +67,19 @@ namespace reach */ bool run(const StudyParameters &sp); + std::shared_ptr get_node(){ + + if (!node_.get()) + { + throw std::runtime_error("Node hasn't been initialized yet!"); + } + return node_; + } + private: - bool initializeStudy(); + bool initializeStudy(const StudyParameters &sp); - bool getReachObjectPointCloud(const rclcpp::Node::SharedPtr &node); + bool getReachObjectPointCloud(); void runInitialReachStudy(); @@ -62,8 +89,6 @@ namespace reach bool compareDatabases(); - ros::NodeHandle nh_; - StudyParameters sp_; pcl::PointCloud::Ptr cloud_; @@ -85,6 +110,13 @@ namespace reach std::string results_dir_; sensor_msgs::msg::PointCloud2 cloud_msg_; + + std::shared_ptr node_; + rclcpp::Publisher::SharedPtr ps_pub_; + + // robot model + moveit::core::RobotModelConstPtr model_; + }; } // namespace core diff --git a/reach_core/include/reach_core/reach_visualizer.h b/reach_core/include/reach_core/reach_visualizer.h index a1048eca..38f71378 100644 --- a/reach_core/include/reach_core/reach_visualizer.h +++ b/reach_core/include/reach_core/reach_visualizer.h @@ -50,15 +50,15 @@ namespace reach void update(); private: - void reSolveIKCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb); + void reSolveIKCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb); - void showResultCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb); + void showResultCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb); - void showSeedCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb); + void showSeedCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb); - void reachNeighborsDirectCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb); + void reachNeighborsDirectCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb); - void reachNeighborsRecursiveCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb); + void reachNeighborsRecursiveCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb); ReachDatabasePtr db_; diff --git a/reach_core/include/reach_core/study_parameters.h b/reach_core/include/reach_core/study_parameters.h index 3c5e73d1..c3caf73c 100644 --- a/reach_core/include/reach_core/study_parameters.h +++ b/reach_core/include/reach_core/study_parameters.h @@ -18,7 +18,7 @@ #include #include -#include +//#include namespace reach { @@ -49,17 +49,23 @@ struct StudyOptimization */ struct StudyParameters { - XmlRpc::XmlRpcValue ik_solver_config; - XmlRpc::XmlRpcValue display_config; +// XmlRpc::XmlRpcValue ik_solver_config; +// XmlRpc::XmlRpcValue display_config; + std::string ik_solver_config_name; + std::string display_config_name; StudyOptimization optimization; std::string config_name; + std::string results_package; std::string results_directory; - std::string pcd_filename; + std::string pcd_package; + std::string pcd_filename_path; bool visualize_results; bool get_neighbors; std::vector compare_dbs; std::string fixed_frame; std::string object_frame; + std::string planning_group; + bool run_initial_study_only; }; } // namespace core diff --git a/reach_core/include/reach_core/utils/serialization_utils.h b/reach_core/include/reach_core/utils/serialization_utils.h index 3f8e099e..aefa651b 100644 --- a/reach_core/include/reach_core/utils/serialization_utils.h +++ b/reach_core/include/reach_core/utils/serialization_utils.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace reach { @@ -31,12 +32,9 @@ namespace reach bool toFile(const std::string &path, const T &msg) { - namespace ser = ros::serialization; - uint32_t serialize_size = ser::serializationLength(msg); - boost::shared_array buffer(new uint8_t[serialize_size]); - - ser::OStream stream(buffer.get(), serialize_size); - ser::serialize(stream, msg); + auto serializer = rclcpp::Serialization(); + rclcpp::SerializedMessage ser_msg; + serializer.serialize_message(&msg, &ser_msg); std::ofstream file(path.c_str(), std::ios::out | std::ios::binary); if (!file) @@ -45,7 +43,7 @@ namespace reach } else { - file.write((char *)buffer.get(), serialize_size); + file.write((char *)ser_msg.get_rcl_serialized_message().buffer, ser_msg.capacity()); return file.good(); } } @@ -54,11 +52,11 @@ namespace reach bool fromFile(const std::string &path, T &msg) { - namespace ser = ros::serialization; - +// RCLCPP_INFO(rclcpp::get_logger("serialization_utils"), "Serializing from file..."); std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary); if (!ifs) { + RCLCPP_INFO(rclcpp::get_logger("serialization_utils"), "Stream '%s' does not exist!", path.c_str()); return false; } @@ -69,10 +67,19 @@ namespace reach uint32_t file_size = end - begin; - boost::shared_array ibuffer(new uint8_t[file_size]); + std::shared_ptr ibuffer(new uint8_t[file_size]); ifs.read((char *)ibuffer.get(), file_size); - ser::IStream istream(ibuffer.get(), file_size); - ser::deserialize(istream, msg); + +// for(size_t i=0; iget_rcl_serialized_message().buffer = ibuffer.get(); + ser_msg->get_rcl_serialized_message().buffer_length = file_size; + ser_msg->get_rcl_serialized_message().buffer_capacity = file_size * sizeof (uint8_t); + auto serializer = rclcpp::Serialization(); + serializer.deserialize_message(ser_msg, &msg); +// RCLCPP_INFO(rclcpp::get_logger("serialization_utils"), "Successfully serialized from file!"); return true; } diff --git a/reach_core/include/reach_core/utils/visualization_utils.h b/reach_core/include/reach_core/utils/visualization_utils.h index ad1b688d..e74ebd71 100644 --- a/reach_core/include/reach_core/utils/visualization_utils.h +++ b/reach_core/include/reach_core/utils/visualization_utils.h @@ -17,62 +17,63 @@ #define REACH_UTILS_VISUALIZATION_UTILS_H #include -// #include #include #include #include -#include namespace reach { namespace utils { - /** - * @brief makeInteractiveMarker - * @param r - * @param frame - * @param scale - * @return - */ + /** + * @brief makeInteractiveMarker + * @param r + * @param frame + * @param scale + * @return + */ visualization_msgs::msg::Marker - makeVisual(const reach_msgs::msg::ReachRecord &r, + makeVisual(const rclcpp::Node::SharedPtr &node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale, const std::string &ns = "reach", const boost::optional> &color = {}); /** - * @brief makeInteractiveMarker - * @param r - * @param frame - * @param scale - * @return - */ + * @brief makeInteractiveMarker + * @param r + * @param frame + * @param scale + * @return + */ visualization_msgs::msg::InteractiveMarker - makeInteractiveMarker(const reach_msgs::msg::ReachRecord &r, + makeInteractiveMarker(const rclcpp::Node::SharedPtr &node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale); /** - * @brief makeMarker - * @param pts - * @param frame - * @param scale - * @param ns - * @return - */ + * @brief makeMarker + * @param pts + * @param frame + * @param scale + * @param ns + * @return + */ visualization_msgs::msg::Marker - makeMarker(const std::vector &pts, + makeMarker(const rclcpp::Node::SharedPtr &node, + const std::vector &pts, const std::string &frame, const double scale, const std::string &ns = ""); /** - * @brief getMajorLength - * @param cloud - * @return - */ + * @brief getMajorLength + * @param cloud + * @return + */ double getMajorLength(pcl::PointCloud::Ptr cloud); } // namespace utils diff --git a/reach_core/launch/setup.launch.py b/reach_core/launch/setup.launch.py new file mode 100644 index 00000000..aa1e62de --- /dev/null +++ b/reach_core/launch/setup.launch.py @@ -0,0 +1,38 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + # Command, + # FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from launch.conditions import IfCondition + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "visualize_results", + description="Package to look for study parameters yaml file.", + default_value="true" + ) + ) + + visualize_results = LaunchConfiguration("visualize_results") + + load_point_cloud_server_node = Node( + package="reach_core", + executable="load_point_cloud_server_node", + name="load_point_cloud_server_node", + output="screen", + parameters=[] + ) + + nodes_to_run = [load_point_cloud_server_node, + ] + + return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/launch/start.launch.py b/reach_core/launch/start.launch.py new file mode 100644 index 00000000..bc6c640d --- /dev/null +++ b/reach_core/launch/start.launch.py @@ -0,0 +1,220 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + # Command, + # FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +import os +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "parameters_package", + description="Package to look for study parameters yaml file.", + default_value="reach_demo" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "parameters_filename", + description="YAML file for study parameters.", + default_value="params.yaml" + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", + default_value="true", + description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument("xacro_file", + default_value="reach_study.xacro", + description="Xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument("moveit_config_file", + default_value="reach_study.srdf.xacro", + description="Moveit config xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + + parameters_package = LaunchConfiguration("parameters_package") + parameters_filename = LaunchConfiguration("parameters_filename") + moveit_config_file = LaunchConfiguration("moveit_config_file") + controllers_file = LaunchConfiguration("controllers_file") + + study_parameters = PathJoinSubstitution( + [FindPackageShare(parameters_package), "config", parameters_filename] + ) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("reach_demo"), "model", LaunchConfiguration("xacro_file")] + ), + ] + ) + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("reach_demo"), "model", moveit_config_file] + ), + ] + ) + + controllers = PathJoinSubstitution( + [FindPackageShare(parameters_package), "model/motoman_sia20d/config", controllers_file] + ) + + kinematics_yaml = load_yaml("reach_demo", "model/motoman_sia20d/config/kinematics.yaml") + robot_description = {"robot_description": robot_description_content} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} + + robot_reach_study_node = Node( + package="reach_core", + executable="robot_reach_study_node", + name="robot_reach_study_node", + output="screen", + parameters=[ + study_parameters, + robot_description, + robot_description_semantic, + robot_description_kinematics + ], + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, controllers], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + "reach_demo", "model/motoman_sia20d/config/moveit_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "reach_demo", "model/motoman_sia20d/config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + ], + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2_moveit", + output="log", + # arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + robot_description_kinematics, + # robot_description_planning, + ], + ) + + nodes_to_run = [robot_reach_study_node, + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + run_move_group_node, + rviz_node] + + return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/launch/start_demo.launch.py b/reach_core/launch/start_demo.launch.py new file mode 100644 index 00000000..97114aee --- /dev/null +++ b/reach_core/launch/start_demo.launch.py @@ -0,0 +1,223 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + # Command, + # FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory +import os +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "parameters_package", + description="Package to look for study parameters yaml file.", + default_value="reach_demo" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "parameters_filename", + description="YAML file for study parameters.", + default_value="params.yaml" + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", + default_value="true", + description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument("xacro_file", + default_value="reach_study.xacro", + description="Xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument("moveit_config_file", + default_value="reach_study.srdf.xacro", + description="Moveit config xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument( + "controllers_file", + default_value="controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + + parameters_package = LaunchConfiguration("parameters_package") + parameters_filename = LaunchConfiguration("parameters_filename") + moveit_config_file = LaunchConfiguration("moveit_config_file") + controllers_file = LaunchConfiguration("controllers_file") + + study_parameters = PathJoinSubstitution( + [FindPackageShare(parameters_package), "config", parameters_filename] + ) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("reach_demo"), "model", LaunchConfiguration("xacro_file")] + ), + ] + ) + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("reach_demo"), "model", moveit_config_file] + ), + ] + ) + + controllers = PathJoinSubstitution( + [FindPackageShare(parameters_package), "model/motoman_sia20d/config", controllers_file] + ) + + kinematics_yaml = load_yaml("reach_demo", "model/motoman_sia20d/config/kinematics.yaml") + robot_description = {"robot_description": robot_description_content} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} + + robot_reach_study_node = Node( + package="reach_core", + executable="robot_reach_study_node", + name="robot_reach_study_node", + output="screen", + parameters=[ + study_parameters, + robot_description, + robot_description_semantic, + robot_description_kinematics + ], + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, controllers], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + "reach_demo", "model/motoman_sia20d/config/moveit_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + "reach_demo", "model/motoman_sia20d/config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Start the actual move_group node/action server + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + ], + ) + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("reach_core"), "rviz", "reach_study_config.rviz"] + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2_moveit", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + robot_description_kinematics, + # robot_description_planning, + ], + ) + + nodes_to_run = [robot_reach_study_node, + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + run_move_group_node, + rviz_node] + + return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/package.xml b/reach_core/package.xml index 1b422ab7..b455b30a 100644 --- a/reach_core/package.xml +++ b/reach_core/package.xml @@ -13,24 +13,24 @@ https://github.com/ros-industrial/reach/issues https://github.com/ros-industrial/reach/ - ament_cmake + ament_cmake_ros geometry_msgs interactive_markers moveit_core pcl_conversions - + pluginlib rclcpp reach_msgs tf2_ros tf2_eigen - visualization_msgs - - + visualization_msgs + moveit_ros_planning_interface + ament_cmake diff --git a/reach_core/plugin_description.xml b/reach_core/plugin_description.xml index 1e1cc6ce..d2bd5439 100644 --- a/reach_core/plugin_description.xml +++ b/reach_core/plugin_description.xml @@ -1,4 +1,4 @@ - + diff --git a/reach_core/rviz/reach_study_config.rviz b/reach_core/rviz/reach_study_config.rviz new file mode 100644 index 00000000..e91d13f0 --- /dev/null +++ b/reach_core/rviz/reach_study_config.rviz @@ -0,0 +1,466 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MarkerArray1/Topic1 + - /Marker1/Topic1 + - /MarkerArray2/Topic1 + - /TF1/Frames1 + - /PlanningScene1 + - /PlanningScene1/Scene Geometry1 + Splitter Ratio: 0.5 + Tree Height: 771 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.5142728090286255 + Min Value: 0.2139683961868286 + Value: true + Axis: Z + Channel Name: normal_z + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.9990596175193787 + Min Color: 0; 0; 0 + Min Intensity: -0.9988572597503662 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: input_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /reach_comparison + Value: false + - Class: rviz_default_plugins/InteractiveMarkers + Enable Transparency: false + Enabled: true + Interactive Markers Namespace: /reach_int_markers + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Value: true + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: System Default + Reliability Policy: Reliable + Value: /reach_neighbors + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /reach_neighbors_array + Value: false + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 1 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: moveit_rviz_plugin/PlanningScene + Enabled: true + Move Group Namespace: "" + Name: PlanningScene + Planning Scene Topic: /planning_scene_display + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_b: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_e: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_l: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_r: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_s: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_t: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_u: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + reach_object: + Alpha: 1 + Show Axes: false + Show Trail: false + tcp: + Alpha: 1 + Show Axes: false + Show Trail: false + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: false + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: dermatoscope + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: false + Velocity_Scaling_Factor: 0.1 + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.004999999888241291 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pose_stamped + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /display_contacts + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /display_cost_sources + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.004999999888241291 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pose_stamped + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 4.135742664337158 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.4049539566040039 + Y: 0.11036305129528046 + Z: 1.1591986417770386 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5002043843269348 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.6823487281799316 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1129 + Hide Left Dock: false + Hide Right Dock: true + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c7000003a7fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000003a7000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005001000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000215000001d6000001ac01000003fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000039b0000005000000050010000030000000100000110000003a8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000044000003a8000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000056fc0100000002fb0000000800540069006d0065010000000000000780000002ad01000003fb0000000800540069006d00650100000000000004500000000000000000000005b8000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 0 diff --git a/reach_core/src/core/ik_helper.cpp b/reach_core/src/core/ik_helper.cpp index 67ae9a57..5cd3c754 100644 --- a/reach_core/src/core/ik_helper.cpp +++ b/reach_core/src/core/ik_helper.cpp @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. */ -#include +#include #include namespace reach @@ -116,7 +116,7 @@ namespace reach // Use current point's IK solution as seed std::vector new_solution; - boost::optional score = solver->solveIKFromSeed(target, previous_solution, new_solution); + std::optional score = solver->solveIKFromSeed(target, previous_solution, new_solution); if (score) { @@ -184,7 +184,7 @@ namespace reach tf2::fromMsg(neighbors[i].goal, target); // Use current point's IK solution as seed - boost::optional score = solver->solveIKFromSeed(target, current_pose_map, new_pose); + std::optional score = solver->solveIKFromSeed(target, current_pose_map, new_pose); if (score) { // Calculate the joint distance between the seed and new goal states diff --git a/reach_core/src/core/reach_database.cpp b/reach_core/src/core/reach_database.cpp index b489c576..c2091f60 100644 --- a/reach_core/src/core/reach_database.cpp +++ b/reach_core/src/core/reach_database.cpp @@ -92,9 +92,9 @@ namespace reach reach_msgs::msg::ReachDatabase msg; if (!reach::utils::fromFile(filename, msg)) { + RCLCPP_ERROR(LOGGER, "Unable to serialize from file '%s'!", filename.c_str()); return false; } - std::lock_guard lock{mutex_}; for (const auto &r : msg.records) @@ -109,7 +109,7 @@ namespace reach return true; } - boost::optional ReachDatabase::get(const std::string &id) const + std::optional ReachDatabase::get(const std::string &id) const { std::lock_guard lock{mutex_}; auto it = map_.find(id); @@ -168,8 +168,8 @@ namespace reach RCLCPP_INFO_STREAM(LOGGER, "Percent Reached = " << results_.reach_percentage); RCLCPP_INFO_STREAM(LOGGER, "Total points score = " << results_.total_pose_score); RCLCPP_INFO_STREAM(LOGGER, "Normalized total points score = " << results_.norm_total_pose_score); - RCLCPP_INFO_STREAM(LOGGER, "Average reachable neighbors = " << results_.avg_num_neighbors); - RCLCPP_INFO_STREAM(LOGGER, "Average joint distance = " << results_.avg_joint_distance); +// RCLCPP_INFO_STREAM(LOGGER, "Average reachable neighbors = " << results_.avg_num_neighbors); +// RCLCPP_INFO_STREAM(LOGGER, "Average joint distance = " << results_.avg_joint_distance); RCLCPP_INFO_STREAM(LOGGER, "------------------------------------------------"); } diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 24f927b8..28473604 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -21,16 +21,22 @@ #include #include -#include -#include -// #include -#include +#include +#include +#include -const static std::string SAMPLE_MESH_SRV_TOPIC = "sample_mesh"; +#include + +#include +#include +#include + + +constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesh"; const static double SRV_TIMEOUT = 5.0; -const static std::string INPUT_CLOUD_TOPIC = "input_cloud"; -const static std::string SAVED_DB_NAME = "reach.db"; -const static std::string OPT_SAVED_DB_NAME = "optimized_reach.db"; +constexpr char INPUT_CLOUD_TOPIC[] = "input_cloud"; +constexpr char SAVED_DB_NAME[] = "reach.db"; +constexpr char OPT_SAVED_DB_NAME[] = "optimized_reach.db"; namespace reach { @@ -40,62 +46,84 @@ namespace reach { const rclcpp::Logger LOGGER = rclcpp::get_logger("reach_core.reach_visualizer"); } - static const std::string PACKAGE = "reach_core"; - static const std::string IK_BASE_CLASS = "reach::plugins::IKSolverBase"; - static const std::string DISPLAY_BASE_CLASS = "reach::plugins::DisplayBase"; - - ReachStudy::ReachStudy(const ros::NodeHandle &nh) - : nh_(nh), cloud_(new pcl::PointCloud()), db_(new ReachDatabase()), solver_loader_(PACKAGE, IK_BASE_CLASS), display_loader_(PACKAGE, DISPLAY_BASE_CLASS) + constexpr char PACKAGE[] = "reach_core"; + constexpr char IK_BASE_CLASS[] = "reach::plugins::IKSolverBase"; + constexpr char DISPLAY_BASE_CLASS[] = "reach::plugins::DisplayBase"; + + ReachStudy::ReachStudy(const rclcpp::Node::SharedPtr node) + : node_(node), + cloud_(new pcl::PointCloud()), + db_(new ReachDatabase()), + solver_loader_(PACKAGE, IK_BASE_CLASS), + display_loader_(PACKAGE, DISPLAY_BASE_CLASS) { + + } + ReachStudy::~ReachStudy(){ + ik_solver_.reset(); + display_.reset(); + } - bool ReachStudy::initializeStudy() + bool ReachStudy::initializeStudy(const StudyParameters &sp) { ik_solver_.reset(); display_.reset(); + // create robot model shared ptr + model_ = moveit::planning_interface::getSharedRobotModel(node_, "robot_description"); + + ps_pub_ = node_->create_publisher("pose_stamped", 1); try { - ik_solver_ = solver_loader_.createInstance(sp_.ik_solver_config["name"]); - display_ = display_loader_.createInstance(sp_.display_config["name"]); + ik_solver_ = solver_loader_.createSharedInstance(sp_.ik_solver_config_name); + display_ = display_loader_.createSharedInstance(sp_.display_config_name); } - catch (const XmlRpc::XmlRpcException &ex) + catch (const pluginlib::PluginlibException &ex) { - ROS_ERROR_STREAM(ex.getMessage()); + RCLCPP_ERROR(LOGGER, "Pluginlib exception thrown while creating shared instances of ik solver and/or display: '%s'", ex.what()); + ik_solver_.reset(); + display_.reset(); return false; } - catch (const pluginlib::PluginlibException &ex) + catch (const std::exception &ex) { - ROS_ERROR_STREAM(ex.what()); - return false; + RCLCPP_ERROR(LOGGER, "Error while creating shared instances of ik solver and/or display: '%s'", ex.what()); + ik_solver_.reset(); + display_.reset(); + return false; } // Initialize the IK solver plugin and display plugin - if (!ik_solver_->initialize(sp_.ik_solver_config) || - !display_->initialize(sp_.display_config)) + if (!ik_solver_->initialize(sp_.ik_solver_config_name, node_, model_) || + !display_->initialize(sp_.display_config_name, node_, model_)) { + RCLCPP_ERROR(LOGGER, "Could not initialized both display and ik solver plugins!"); + ik_solver_.reset(); + display_.reset(); return false; } display_->showEnvironment(); // Create a directory to store results of study - if (!sp_.results_directory.empty() && boost::filesystem::exists(sp_.results_directory.c_str())) + std::string tmp_dir = ament_index_cpp::get_package_share_directory(sp_.results_package) + "/" + sp_.results_directory; + if (!tmp_dir.empty() && std::filesystem::exists(tmp_dir.c_str())) { - dir_ = sp_.results_directory + "/"; + dir_ = tmp_dir + "/"; } else { - dir_ = ros::package::getPath("reach_core") + "/results/"; - ROS_WARN("Using default results file directory: %s", dir_.c_str()); + dir_ = ament_index_cpp::get_package_share_directory("reach_core") + "/results/"; + RCLCPP_WARN(LOGGER, "Using default results file directory: '%s'", dir_.c_str()); } results_dir_ = dir_ + sp_.config_name + "/"; const char *char_dir = results_dir_.c_str(); - if (!boost::filesystem::exists(char_dir)) + if (!std::filesystem::exists(char_dir)) { - boost::filesystem::path path(char_dir); - boost::filesystem::create_directory(path); + std::filesystem::path path(char_dir); + std::filesystem::create_directory(path); } return true; @@ -103,11 +131,11 @@ namespace reach bool ReachStudy::run(const StudyParameters &sp) { - // Overrwrite the old study parameters + // Overwrite the old study parameters sp_ = sp; // Initialize the study - if (!initializeStudy()) + if (!initializeStudy(sp)) { RCLCPP_ERROR(LOGGER, "Failed to initialize the reach study"); return false; @@ -117,14 +145,16 @@ namespace reach if (!getReachObjectPointCloud()) { RCLCPP_ERROR(LOGGER, "Unable to obtain reach object point cloud"); + ik_solver_.reset(); + display_.reset(); return false; } // Show the reach object collision object and reach object point cloud if (sp_.visualize_results) { - ros::Publisher pub = nh_.advertise(INPUT_CLOUD_TOPIC, 1, true); - pub.publish(cloud_msg_); + rclcpp::Publisher::SharedPtr pub = node_->create_publisher(INPUT_CLOUD_TOPIC, 1); + pub->publish(cloud_msg_); } // Create markers @@ -133,6 +163,7 @@ namespace reach // Attempt to load previously saved optimized reach_study database if (!db_->load(results_dir_ + OPT_SAVED_DB_NAME)) { + RCLCPP_INFO(LOGGER, "Unable to load optimized database at '%s'!",(results_dir_ + OPT_SAVED_DB_NAME).c_str()); // Attempt to load previously saved initial reach study database if (!db_->load(results_dir_ + SAVED_DB_NAME)) { @@ -144,6 +175,10 @@ namespace reach runInitialReachStudy(); db_->printResults(); visualizer_->update(); + // check if we don't have to optimize + if (sp.run_initial_study_only) { + return true; + } } else { @@ -153,6 +188,11 @@ namespace reach db_->printResults(); visualizer_->update(); + // check if we don't have to optimize + if (sp.run_initial_study_only) { + return true; + } + } // Create an efficient search tree for doing nearest neighbors search @@ -203,44 +243,62 @@ namespace reach { if (!compareDatabases()) { - ROS_ERROR("Unable to compare the current reach study database with the other specified databases"); + RCLCPP_ERROR(LOGGER, "Unable to compare the current reach study database with the other specified databases"); } } } + ik_solver_.reset(); + display_.reset(); + return true; } - bool ReachStudy::getReachObjectPointCloud(const rclcpp::Node::SharedPtr &node) + bool ReachStudy::getReachObjectPointCloud() { // Call the sample mesh service to create a point cloud of the reach object mesh - auto client = node->create_client(SAMPLE_MESH_SRV_TOPIC); + auto callback_group_input_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC, rmw_qos_profile_services_default, callback_group_input_); - reach_msgs::srv::LoadPointCloud srv; - srv.request.cloud_filename = sp_.pcd_filename; - srv.request.fixed_frame = sp_.fixed_frame; - srv.request.object_frame = sp_.object_frame; + auto req = std::make_shared(); + req->cloud_filename = ament_index_cpp::get_package_share_directory(sp_.pcd_package) + "/" + sp_.pcd_filename_path; + req->fixed_frame = sp_.fixed_frame; + req->object_frame = sp_.object_frame; - client.waitForExistence(ros::Duration(SRV_TIMEOUT)); - if (!client.call(srv)) - { - RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client.getService() << "'"); - return false; - } - else if (!srv.response.success) - { - RCLCPP_ERROR_STREAM(LOGGER, srv.response.message); - return false; - } + RCLCPP_INFO(LOGGER, "Waiting for service '%s'.", SAMPLE_MESH_SRV_TOPIC); + client->wait_for_service(); + bool success_tmp = false; + bool inner_callback_finished = false; + + auto inner_client_callback = [&,this](rclcpp::Client::SharedFuture inner_future) + { + success_tmp = inner_future.get()->success; + cloud_msg_ = inner_future.get()->cloud; + RCLCPP_INFO(LOGGER, "Inner service callback message: '%s'", inner_future.get()->message.c_str()); + inner_callback_finished = true; + }; + auto inner_future_result = client->async_send_request(req, inner_client_callback); + + // quick fix to wait for inner callback to finish + //TODO(livanov93) Add visible flag within the inner callback + while(!inner_callback_finished) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } - cloud_msg_ = srv.response.cloud; - pcl::fromROSMsg(cloud_msg_, *cloud_); + if (success_tmp){ + pcl::fromROSMsg(cloud_msg_, *cloud_); - cloud_msg_.header.frame_id = sp_.fixed_frame; - cloud_msg_.header.stamp = node->now(); + cloud_msg_.header.frame_id = sp_.fixed_frame; + cloud_msg_.header.stamp = node_->now(); - return true; - } + return true; + + } else { + RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client->get_service_name() + << "'"); + return false; + } + } void ReachStudy::runInitialReachStudy() { @@ -268,7 +326,7 @@ namespace reach // Solve IK std::vector solution; - boost::optional score = ik_solver_->solveIKFromSeed(tgt_frame, jointStateMsgToMap(seed_state), solution); + std::optional score = ik_solver_->solveIKFromSeed(tgt_frame, jointStateMsgToMap(seed_state), solution); // Create objects to save in the reach record geometry_msgs::msg::Pose tgt_pose; @@ -278,6 +336,20 @@ namespace reach if (score) { + geometry_msgs::msg::PoseStamped tgt_pose_stamped; + tgt_pose_stamped.pose = tgt_pose; + tgt_pose_stamped.header.frame_id = cloud_msg_.header.frame_id; + ps_pub_->publish(tgt_pose_stamped); + + std::map robot_configuration; + // create map + std::transform(goal_state.name.begin(), goal_state.name.end(), solution.begin(), std::inserter(robot_configuration, robot_configuration.end()), + [](std::string &jname, double jvalue) + { + return std::make_pair(jname, jvalue); + }); + + display_->updateRobotPose(robot_configuration); goal_state.position = solution; auto msg = makeRecord(std::to_string(i), true, tgt_pose, seed_state, goal_state, *score); db_->put(msg); @@ -364,8 +436,7 @@ namespace reach current_counter = previous_pct = neighbor_count = 0; std::atomic total_joint_distance; const int total = db_->size(); - -// Iterate + // Iterate #pragma parallel for for (auto it = db_->begin(); it != db_->end(); ++it) { @@ -373,8 +444,7 @@ namespace reach if (msg.reached) { NeighborReachResult result; - reachNeighborsRecursive(db_, msg, ik_solver_, sp_.optimization.radius, result); //, search_tree_); - + reachNeighborsRecursive(db_, msg, ik_solver_, sp_.optimization.radius, result, search_tree_); neighbor_count += static_cast(result.reached_pts.size() - 1); total_joint_distance = total_joint_distance + result.joint_distance; } diff --git a/reach_core/src/core/reach_visualizer.cpp b/reach_core/src/core/reach_visualizer.cpp index 9c8f1729..a1678bca 100644 --- a/reach_core/src/core/reach_visualizer.cpp +++ b/reach_core/src/core/reach_visualizer.cpp @@ -17,7 +17,9 @@ #include #include #include -#include +#include + +#include namespace reach { @@ -28,6 +30,8 @@ namespace reach const rclcpp::Logger LOGGER = rclcpp::get_logger("reach_core.reach_visualizer"); } + using std::placeholders::_1; + ReachVisualizer::ReachVisualizer(ReachDatabasePtr db, reach::plugins::IKSolverBasePtr solver, reach::plugins::DisplayBasePtr display, @@ -39,11 +43,11 @@ namespace reach using CBType = interactive_markers::MenuHandler::FeedbackCallback; using FBType = visualization_msgs::msg::InteractiveMarkerFeedback; - CBType show_result_cb = boost::bind(&ReachVisualizer::showResultCB, this, _1); - CBType show_seed_cb = boost::bind(&ReachVisualizer::showSeedCB, this, _1); - CBType re_solve_ik_cb = boost::bind(&ReachVisualizer::reSolveIKCB, this, _1); - CBType neighbors_direct_cb = boost::bind(&ReachVisualizer::reachNeighborsDirectCB, this, _1); - CBType neighbors_recursive_cb = boost::bind(&ReachVisualizer::reachNeighborsRecursiveCB, this, _1); + CBType show_result_cb = std::bind(&ReachVisualizer::showResultCB, this, _1); + CBType show_seed_cb = std::bind(&ReachVisualizer::showSeedCB, this, _1); + CBType re_solve_ik_cb = std::bind(&ReachVisualizer::reSolveIKCB, this, _1); + CBType neighbors_direct_cb = std::bind(&ReachVisualizer::reachNeighborsDirectCB, this, _1); + CBType neighbors_recursive_cb = std::bind(&ReachVisualizer::reachNeighborsRecursiveCB, this, _1); display_->createMenuFunction("Show Result", show_result_cb); display_->createMenuFunction("Show Seed Position", show_seed_cb); @@ -60,9 +64,9 @@ namespace reach display_->addInteractiveMarkerData(db_->toReachDatabaseMsg()); } - void ReachVisualizer::reSolveIKCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb) + void ReachVisualizer::reSolveIKCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb) { - boost::optional lookup = db_->get(fb->marker_name); + std::optional lookup = db_->get(fb->marker_name); if (lookup) { const std::vector &seed_pose = lookup->seed_state.position; @@ -78,7 +82,7 @@ namespace reach // Re-solve IK at the selected marker std::vector goal_pose; - boost::optional score = solver_->solveIKFromSeed(target, seed_map, goal_pose); + std::optional score = solver_->solveIKFromSeed(target, seed_map, goal_pose); // Update the database if the IK solution was valid if (score) @@ -107,7 +111,7 @@ namespace reach } } - void ReachVisualizer::showResultCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb) + void ReachVisualizer::showResultCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb) { auto lookup = db_->get(fb->marker_name); if (lookup) @@ -120,7 +124,7 @@ namespace reach } } - void ReachVisualizer::showSeedCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb) + void ReachVisualizer::showSeedCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb) { auto lookup = db_->get(fb->marker_name); if (lookup) @@ -133,7 +137,7 @@ namespace reach } } - void ReachVisualizer::reachNeighborsDirectCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb) + void ReachVisualizer::reachNeighborsDirectCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb) { auto lookup = db_->get(fb->marker_name); if (lookup) @@ -152,7 +156,7 @@ namespace reach } } - void ReachVisualizer::reachNeighborsRecursiveCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&fb) + void ReachVisualizer::reachNeighborsRecursiveCB(const visualization_msgs::msg::InteractiveMarkerFeedback::ConstSharedPtr &fb) { auto lookup = db_->get(fb->marker_name); if (lookup) diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index ee02d25b..45402aee 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -14,31 +14,39 @@ * limitations under the License. */ #include "reach_core/reach_database.h" -#include -#include -#include +#include "rclcpp/rclcpp.hpp" +#include + + +#include #include const static std::string RESULTS_FOLDER_NAME = "results"; -const static std::string OPT_DB_NAME = "optimized_reach.db"; +//const static std::string OPT_DB_NAME = "optimized_reach.db"; +const static std::string OPT_DB_NAME = "reach.db"; + +typedef std::pair coordinate_pair_type; +typedef std::pair coordinate_path; +typedef std::pair coordinate_config; +typedef std::unordered_map> coordinate_config_map_of_vecotors; -bool get_all(const boost::filesystem::path& root, +bool get_all(const std::filesystem::path& root, const std::string& ext, - std::vector>& ret) + std::vector>& ret) { - if(!boost::filesystem::exists(root) || !boost::filesystem::is_directory(root)) return false; + if(!std::filesystem::exists(root) || !std::filesystem::is_directory(root)) return false; - boost::filesystem::recursive_directory_iterator it(root); - boost::filesystem::recursive_directory_iterator endit; + std::filesystem::recursive_directory_iterator it(root); + std::filesystem::recursive_directory_iterator endit; while(it != endit) { - if(boost::filesystem::is_regular_file(*it) && it->path().extension() == ext) + if(std::filesystem::is_regular_file(*it) && it->path().extension() == ext) { // Capture only the optimized reach databases if(it->path().filename() == OPT_DB_NAME) { - std::pair tmp; + std::pair tmp; tmp.first = it->path().parent_path().filename(); tmp.second = it->path(); ret.push_back(tmp); @@ -47,58 +55,224 @@ bool get_all(const boost::filesystem::path& root, ++it; } - std::sort(ret.begin(), ret.end()); + + std::sort(ret.begin(), ret.end(), + [&](const std::pair &first, + std::pair &second){ + + reach::core::ReachDatabase db; + // first + db.load(first.second); + reach::core::StudyResults res = db.getStudyResults(); + float first_reach_percentage = res.reach_percentage; + // second + db.load(second.second); + res = db.getStudyResults(); + float second_reach_percentage = res.reach_percentage; + return first_reach_percentage > second_reach_percentage; + + }); return true; } + int main(int argc, char **argv) { - if(argc > 2) - { - return -1; - } + // Initialize ROS + rclcpp::init(argc, argv); + rclcpp::NodeOptions options(rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)); + // create node + auto node = std::make_shared("data_loader_node", options); + + std::string pkg_name; + std::string dir_name; + bool chk_all_sub_dirs; + bool avg_neighbor_count; + bool print_per_patient_config = false; + bool print_result_total = false; + bool print_per_coordinate = false; + + node->get_parameter_or("package_name", pkg_name, "reach_core"); + node->get_parameter_or("directory_name", dir_name, RESULTS_FOLDER_NAME); + node->get_parameter_or("check_all_subdirectories", chk_all_sub_dirs, false); + node->get_parameter_or("avg_neighbor_count", avg_neighbor_count, false); + node->get_parameter_or("print_total", print_result_total, false); + node->get_parameter_or("print_per_patient_config", print_per_patient_config, false); + node->get_parameter_or("print_per_coordinate", print_per_coordinate, false); + - std::string root_path = ros::package::getPath("reach_core") + "/" + RESULTS_FOLDER_NAME; + std::string root_path = std::string(ament_index_cpp::get_package_share_directory(pkg_name)) + "/" + dir_name; - if(argv[1]) + if(argv[1] && !chk_all_sub_dirs) { const std::string folder_name = argv[1]; root_path += "/" + folder_name; } - boost::filesystem::path root (root_path); - std::vector> files; + std::filesystem::path root (root_path); + std::vector> files; + if(!get_all(root, ".db", files)) { std::cout << "Specified directory does not exist"; return 0; } - std::cout << boost::format("%-30s %=25s %=25s %=25s %=25s\n") - % "Configuration Name" - % "Reach Percentage" - % "Normalized Total Pose Score" - % "Average Reachable Neighbors" - % "Average Joint Distance"; + // specific printing + if(print_per_patient_config) { + std::map>>> tmp_storage; + for (size_t i = 0; i < files.size(); ++i) { + const std::string config = files[i].first.string(); + const std::string path = files[i].second.string(); + size_t idx_start = config.find('(') + 1; + size_t idx_end = config.find(')') - 1; + std::string config_name_map = config.substr(0, idx_start - 2); - for(size_t i = 0; i < files.size(); ++i) - { - const std::string config = files[i].first.string(); - const std::string path = files[i].second.string(); + reach::core::ReachDatabase db; + if (db.load(path)) { + reach::core::StudyResults res = db.getStudyResults(); + tmp_storage[config_name_map].push_back(std::make_pair(path, std::make_pair(config, res))); + } - reach::core::ReachDatabase db; - if(db.load(path)) - { - reach::core::StudyResults res = db.getStudyResults(); - std::cout << boost::format("%-30s %=25.3f %=25.6f %=25.3f %=25.3f\n") - % config.c_str() - % res.reach_percentage - % res.norm_total_pose_score - % res.avg_num_neighbors - % res.avg_joint_distance; + } + + for (auto it = tmp_storage.begin(); it != tmp_storage.end(); it++) { + + std::sort(it->second.begin(), it->second.end(), + [&](std::pair> &first, + std::pair> &second) { + + reach::core::ReachDatabase db; + // first + db.load(first.first); + reach::core::StudyResults res = db.getStudyResults(); + float first_reach_percentage = res.reach_percentage; + // second + db.load(second.first); + res = db.getStudyResults(); + float second_reach_percentage = res.reach_percentage; + return first_reach_percentage > second_reach_percentage; + + }); + } + + + for (auto it = tmp_storage.begin(); it != tmp_storage.end(); it++) { + std::cout << boost::format("----------------------------------------------------------------------------\n")<first; + for (auto &iter: it->second) { + std::cout << boost::format("%-60s %=25.3f %=25.6f\n") + % iter.second.first + % iter.second.second.reach_percentage + % iter.second.second.norm_total_pose_score; + } + } } - } + + if(print_per_coordinate) { + std::map>>> tmp_storage; + for (size_t i = 0; i < files.size(); ++i) { + const std::string config = files[i].first.string(); + const std::string path = files[i].second.string(); + size_t idx_start = config.find('(') + 1; + size_t idx_end = config.find(')') - 1; + std::string config_name_map = config.substr(idx_start, idx_end); + + reach::core::ReachDatabase db; + if (db.load(path)) { + reach::core::StudyResults res = db.getStudyResults(); + tmp_storage[config_name_map].push_back(std::make_pair(path, std::make_pair(config, res))); + } + + } + + std::map per_coordinate_percentage; + for (auto it = tmp_storage.begin(); it != tmp_storage.end(); it++) { + + per_coordinate_percentage[it->first] = 0.0; + for(auto &item: it->second){ + per_coordinate_percentage[it->first] += item.second.second.reach_percentage; + } + per_coordinate_percentage[it->first] /= it->second.size(); + + } + std::vector> per_coordinate_percenage_vec(per_coordinate_percentage.begin(), per_coordinate_percentage.end()); + + std::sort(per_coordinate_percenage_vec.begin(), per_coordinate_percenage_vec.end(), + [&](const std::pair &first, + std::pair &second){ + + return first.second > second.second; + + }); + + std::cout << boost::format("----------------------------------------------------------------------------\n")<first + % it->second; + } + } + + if(print_result_total) { + if (avg_neighbor_count) { + std::cout << boost::format("%-60s %=25s %=25s %=25s %=25s\n") + % "Configuration Name" + % "Reach Percentage" + % "Normalized Total Pose Score" + % "Average Reachable Neighbors" + % "Average Joint Distance"; + } else { + std::cout << boost::format("%-60s %=25s %=25s\n") + % "Configuration Name" + % "Reach Percentage" + % "Normalized Total Pose Score"; + } + + + for (size_t i = 0; i < files.size(); ++i) { + const std::string config = files[i].first.string(); + const std::string path = files[i].second.string(); + + reach::core::ReachDatabase db; + if (db.load(path)) { + reach::core::StudyResults res = db.getStudyResults(); + if (avg_neighbor_count) { + std::cout << boost::format("%-60s %=25.3f %=25.6f %=25.3f %=25.3f\n") + % config.c_str() + % res.reach_percentage + % res.norm_total_pose_score + % res.avg_num_neighbors + % res.avg_joint_distance; + } else { + std::cout << boost::format("%-60s %=25.3f %=25.6f\n") + % config.c_str() + % res.reach_percentage + % res.norm_total_pose_score; + } + } + } + } + + // shutdown + rclcpp::shutdown(); return 0; } diff --git a/reach_core/src/load_point_cloud_server_node.cpp b/reach_core/src/load_point_cloud_server_node.cpp index 417b6c90..9f176d3f 100644 --- a/reach_core/src/load_point_cloud_server_node.cpp +++ b/reach_core/src/load_point_cloud_server_node.cpp @@ -13,111 +13,143 @@ * See the License for the specific language governing permissions and * limitations under the License. */ + +#include + #include #include #include -#include -#include -#include +//#include +#include +#include +#include #include -#include - -const static std::string SAMPLE_MESH_SRV_TOPIC = "sample_mesh"; - -bool hasNormals(pcl::PCLPointCloud2 &cloud) -{ - auto nx = std::find_if(cloud.fields.begin(), cloud.fields.end(), [](pcl::PCLPointField &field) - { return field.name == "normal_x"; }); - auto ny = std::find_if(cloud.fields.begin(), cloud.fields.end(), [](pcl::PCLPointField &field) - { return field.name == "normal_y"; }); - auto nz = std::find_if(cloud.fields.begin(), cloud.fields.end(), [](pcl::PCLPointField &field) - { return field.name == "normal_z"; }); - - if (nx == cloud.fields.end() || ny == cloud.fields.end() || nz == cloud.fields.end()) - { - return false; - } - else - { - return true; - } -} - -bool getSampledMesh(reach_msgs::LoadPointCloudRequest &req, - reach_msgs::LoadPointCloudResponse &res) -{ - // Check if file exists - if (!boost::filesystem::exists(req.cloud_filename)) - { - res.message = "File '" + req.cloud_filename + "' does not exist"; - res.success = false; - - return true; - } - - pcl::PCLPointCloud2 cloud_msg; - if (pcl::io::loadPCDFile(req.cloud_filename, cloud_msg) == -1) - { - res.message = "Unable to load point cloud from '" + req.cloud_filename + "'"; - res.success = false; - return true; - } - - if (!hasNormals(cloud_msg)) - { - res.message = "Point cloud file does not contain normals. Please regenerate the cloud with " - "normal vectors"; - res.success = false; - return true; - } - - pcl::PointCloud cloud; - pcl::fromPCLPointCloud2(cloud_msg, cloud); - - // Transform point cloud to correct frame - tf2_ros::Buffer buffer; - tf2_ros::TransformListener listener(buffer); - Eigen::Isometry3d transform; - try - { - geometry_msgs::TransformStamped tf = buffer.lookupTransform(req.fixed_frame, - req.object_frame, - ros::Time(0), - ros::Duration(5.0)); - transform = tf2::transformToEigen(tf.transform); - } - catch (const tf2::TransformException &ex) - { - res.message = ex.what(); - res.success = false; - return true; - } - - pcl::PointCloud transformed_cloud; - pcl::transformPointCloudWithNormals(cloud, transformed_cloud, transform.matrix()); - - // Convert point cloud to message for output - sensor_msgs::PointCloud2 msg; - pcl::toROSMsg(transformed_cloud, res.cloud); - - res.success = true; - res.message = "Successfully loaded point cloud from '" + req.cloud_filename + "'"; - - return true; -} +#include + +constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesh"; + +using LoadPCLSrv = reach_msgs::srv::LoadPointCloud; +using LoadPCLReq = reach_msgs::srv::LoadPointCloud_Request; +using LoadPCLReqSharedPtr = LoadPCLReq::SharedPtr; +using LoadPCLRes = reach_msgs::srv::LoadPointCloud_Response; +using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; + + + class PointCloudServerNode : public rclcpp::Node { + public: + explicit PointCloudServerNode(const std::string &node_name) : Node(node_name) { + + server_ = this->create_service(SAMPLE_MESH_SRV_TOPIC, [this](const LoadPCLReqSharedPtr req, + LoadPCLResSharedPtr res){ + + RCLCPP_INFO(this->get_logger(), "Service callback started!"); + + + // getSampledMesh callback + // Check if file exists + if (!std::filesystem::exists(req->cloud_filename)) { + res->message = "File '" + req->cloud_filename + "' does not exist"; + res->success = false; + return false; + } + + pcl::PCLPointCloud2 cloud_msg; + if (pcl::io::loadPCDFile(req->cloud_filename, cloud_msg) == -1) { + res->message = "Unable to load point cloud from '" + req->cloud_filename + "'"; + res->success = false; + return false; + } + + if (!hasNormals(cloud_msg)) { + res->message = "Point cloud file does not contain normals. Please regenerate the cloud with " + "normal vectors"; + res->success = false; + return true; + } + + pcl::PointCloud cloud; + pcl::fromPCLPointCloud2(cloud_msg, cloud); + + // Transform point cloud to correct frame + tf2_ros::Buffer buffer(this->get_clock()); + tf2_ros::TransformListener listener(buffer); + Eigen::Isometry3d transform; + try { + RCLCPP_INFO(this->get_logger(), "Try to look for transform!"); + geometry_msgs::msg::TransformStamped tf = buffer.lookupTransform(req->fixed_frame, + req->object_frame, + rclcpp::Time(0), + rclcpp::Duration::from_seconds(5.0)); + transform = tf2::transformToEigen(tf.transform); + RCLCPP_INFO(this->get_logger(), "x = %f y = %f z = %f ", tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z); + RCLCPP_INFO(this->get_logger(), "qx = %f qy = %f qz = %f qw = %f ", tf.transform.rotation.x, tf.transform.rotation.y, tf.transform.rotation.z, + tf.transform.rotation.w); + + } + catch (const tf2::TransformException &ex) { + RCLCPP_ERROR(this->get_logger(), "Catch tf exception!"); + + res->message = ex.what(); + res->success = false; + RCLCPP_ERROR(this->get_logger(), "'%s'", ex.what()); + return false; + } + catch(const rclcpp::exceptions::RCLError &exerr){ + RCLCPP_ERROR(this->get_logger(), "Catch RCLError exception!"); + + RCLCPP_ERROR(this->get_logger(), "'%s'", exerr.what()); + res->success = false; + res->message = exerr.what(); + return false; + } + + pcl::PointCloud transformed_cloud; + pcl::transformPointCloudWithNormals(cloud, transformed_cloud, transform.matrix()); + + // Convert point cloud to message for output + sensor_msgs::msg::PointCloud2 msg; + pcl::toROSMsg(transformed_cloud, res->cloud); + + res->success = true; + res->message = "Successfully loaded point cloud from '" + req->cloud_filename + "'"; + + RCLCPP_INFO(this->get_logger(), "Service callback finished!"); + + return true; + }); + server_->get_service_name(); + } + + private: + + rclcpp::Service::SharedPtr server_; + + bool hasNormals(pcl::PCLPointCloud2 &cloud) { + auto nx = std::find_if(cloud.fields.begin(), cloud.fields.end(), + [](pcl::PCLPointField &field) { return field.name == "normal_x"; }); + auto ny = std::find_if(cloud.fields.begin(), cloud.fields.end(), + [](pcl::PCLPointField &field) { return field.name == "normal_y"; }); + auto nz = std::find_if(cloud.fields.begin(), cloud.fields.end(), + [](pcl::PCLPointField &field) { return field.name == "normal_z"; }); + + if (nx == cloud.fields.end() || ny == cloud.fields.end() || nz == cloud.fields.end()) { + return false; + } else { + return true; + } + } + + + }; int main(int argc, char **argv) { // Initialize ROS - ros::init(argc, argv, "sample_mesh_server"); - - // Create a ROS node handle - ros::NodeHandle nh; - - // Create a server - ros::ServiceServer service = nh.advertiseService(SAMPLE_MESH_SRV_TOPIC, getSampledMesh); - - ros::spin(); + rclcpp::init(argc, argv); + // create node + auto node = std::make_shared("sample_mesh_server"); + // spin + rclcpp::spin(node); return 0; } diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index 04f18eba..c4844ee9 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -14,9 +14,7 @@ * limitations under the License. */ #include "reach_core/plugins/impl/multiplicative_factory.h" -// #include #include -#include namespace reach { @@ -28,31 +26,33 @@ namespace reach namespace plugins { - const static std::string PACKAGE = "reach_core"; - const static std::string PLUGIN_BASE_NAME = "reach::plugins::EvaluationBase"; + constexpr char PACKAGE[] = "reach_core"; + constexpr char PLUGIN_BASE_NAME[] = "reach::plugins::EvaluationBase"; MultiplicativeFactory::MultiplicativeFactory() : EvaluationBase(), class_loader_(PACKAGE, PLUGIN_BASE_NAME) { } - bool MultiplicativeFactory::initialize(XmlRpc::XmlRpcValue &config) + bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr const node,std::shared_ptr model ) { try { - XmlRpc::XmlRpcValue &plugin_configs = config["plugins"]; + std::vector plugin_configs; + node->get_parameter("ik_solver_config.evaluation_plugin.plugins", plugin_configs); eval_plugins_.reserve(plugin_configs.size()); for (int i = 0; i < plugin_configs.size(); ++i) { - XmlRpc::XmlRpcValue &plugin_config = plugin_configs[i]; - const std::string name = std::string(plugin_config["name"]); + std::string &plugin_config = plugin_configs[i]; + const std::string plugin_name = std::string(plugin_config); + RCLCPP_INFO(LOGGER, "Creating shared instance of plugin '%s'", plugin_name.c_str()); EvaluationBasePtr plugin; try { - plugin = class_loader_.createSharedInstance(name); + plugin = class_loader_.createSharedInstance(plugin_name); } catch (const pluginlib::ClassLoaderException &ex) { @@ -60,7 +60,7 @@ namespace reach continue; } - if (!plugin->initialize(plugin_config)) + if (!plugin->initialize(name, node, model)) { RCLCPP_WARN_STREAM(LOGGER, "Plugin '" << name << "' failed to be initialized; excluding it from the list"); continue; @@ -69,9 +69,9 @@ namespace reach eval_plugins_.push_back(std::move(plugin)); } } - catch (const XmlRpc::XmlRpcException &ex) + catch (const std::exception &ex) { - RCLCPP_ERROR_STREAM(LOGGER, ex.getMessage()); + RCLCPP_ERROR_STREAM(LOGGER, ex.what()); } if (eval_plugins_.empty()) diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 3dfe0126..2593f65b 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -13,71 +13,121 @@ * See the License for the specific language governing permissions and * limitations under the License. */ + +#include #include "reach_core/reach_study.h" #include "reach_core/study_parameters.h" -template -bool get(const ros::NodeHandle& nh, - const std::string& key, - T& val) -{ - if(!nh.getParam(key, val)) - { - ROS_ERROR_STREAM("Failed to get '" << key << "' parameter"); - return false; - } - return true; -} +#include + -bool getStudyParameters(ros::NodeHandle& nh, - reach::core::StudyParameters& sp) +class RobotReachStudyNode : public rclcpp::Node { - if(!get(nh, "config_name", sp.config_name) || - !get(nh, "fixed_frame", sp.fixed_frame) || - !get(nh, "results_directory", sp.results_directory) || - !get(nh, "object_frame", sp.object_frame) || - !get(nh, "pcd_filename", sp.pcd_filename) || - !get(nh, "optimization/radius", sp.optimization.radius) || - !get(nh, "optimization/max_steps", sp.optimization.max_steps) || - !get(nh, "optimization/step_improvement_threshold", sp.optimization.step_improvement_threshold) || - !get(nh, "get_avg_neighbor_count", sp.get_neighbors) || - !get(nh, "compare_dbs", sp.compare_dbs) || - !get(nh, "visualize_results", sp.visualize_results) || - !get(nh, "ik_solver_config", sp.ik_solver_config) || - !get(nh, "display_config", sp.display_config)) - { - return false; - } +public: + explicit RobotReachStudyNode(const std::string& node_name) + : Node(node_name, rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) + { + + } + +public: + bool + getStudyParameters(reach::core::StudyParameters& sp){ + + // fetch parameteres + if (!this->get_parameter("config_name", sp_.config_name) || + !this->get_parameter("fixed_frame", sp_.fixed_frame) || + !this->get_parameter("results_package", sp_.results_package) || + !this->get_parameter("results_directory", sp_.results_directory) || + !this->get_parameter("object_frame", sp_.object_frame) || + !this->get_parameter("pcd_package", sp_.pcd_package) || + !this->get_parameter("pcd_filename_path", sp_.pcd_filename_path) || + !this->get_parameter("planning_group", sp_.planning_group) || + !this->get_parameter("run_initial_study_only", sp_.run_initial_study_only) || + !this->get_parameter("optimization.radius", sp_.optimization.radius) || + !this->get_parameter("optimization.max_steps", sp_.optimization.max_steps) || + !this->get_parameter("optimization.step_improvement_threshold", sp_.optimization.step_improvement_threshold) || + !this->get_parameter("get_avg_neighbor_count", sp_.get_neighbors) || + !this->get_parameter("compare_dbs", sp_.compare_dbs) || + !this->get_parameter("visualize_results", sp_.visualize_results) || + !this->get_parameter("ik_solver_config.name", sp_.ik_solver_config_name) || + !this->get_parameter("display_config.name", sp_.display_config_name) ) { + RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), "One of the main parameters do not exist..." ); + return false; + }else{ + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "config_name: '%s'", sp_.config_name.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "fixed_frame: '%s'", sp_.fixed_frame.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "results_package: '%s'", sp_.results_package.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "results_directory: '%s'", sp_.results_directory.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "object_frame: '%s'", sp_.object_frame.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "pcd_package: '%s'", sp_.pcd_package.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "pcd_filename: '%s'", sp_.pcd_filename_path.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "optimization.radius: '%f'", sp_.optimization.radius ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "optimization.max_steps: '%d'", sp_.optimization.max_steps ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "optimization.step_improvement_threshold: '%f'", sp_.optimization.step_improvement_threshold ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "get_avg_neighbor_count: '%d'", sp_.get_neighbors ); + for (auto const& compare_db: sp_.compare_dbs){ + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "compare_dbs: '%s'", compare_db.c_str() ); + } + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "visualize_results: '%c'", sp_.visualize_results ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "ik_solver_config.name: '%s'", sp_.ik_solver_config_name.c_str() ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "display_config.name: '%s'", sp_.display_config_name.c_str() ); + + if (std::find(sp_.compare_dbs.begin(), sp_.compare_dbs.end(), "") != sp_.compare_dbs.end()){ + sp_.compare_dbs.clear(); + } + + // set params + sp = sp_; + + return true; + } + } + +private: + + reach::core::StudyParameters sp_; + +}; + - return true; -} int main(int argc, char **argv) { - ros::init(argc, argv, "robot_reach_study_node"); - ros::NodeHandle pnh("~"), nh; + // Initialize ROS + rclcpp::init(argc, argv); - ros::AsyncSpinner spinner(1); - spinner.start(); + rclcpp::executors::MultiThreadedExecutor executor; - // Get the study parameters - reach::core::StudyParameters sp; - if(!getStudyParameters(pnh, sp)) - { - return -1; - } + // create node + auto node = std::make_shared("robot_reach_study_node"); + + executor.add_node(node); + + // get the study parameters + reach::core::StudyParameters sp; + if (!node->getStudyParameters(sp)){ + + return -1; + } + + std::thread t1( [&executor]{ + // spin + executor.spin(); + }); - // Initialize the reach study - reach::core::ReachStudy rs (nh); + // Initialize the reach study + reach::core::ReachStudy rs (node); // Run the reach study - if(!rs.run(sp)) + if(!rs.run(sp) || !rclcpp::ok()) { - ROS_ERROR("Unable to perform the reach study"); + RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), "Unable to perform the reach study"); return -1; } - ros::waitForShutdown(); + rclcpp::shutdown(); + t1.join(); return 0; } diff --git a/reach_core/src/utils/visualization_utils.cpp b/reach_core/src/utils/visualization_utils.cpp index b3c32627..0da1f9cd 100644 --- a/reach_core/src/utils/visualization_utils.cpp +++ b/reach_core/src/utils/visualization_utils.cpp @@ -14,7 +14,7 @@ * limitations under the License. */ #include "reach_core/utils/visualization_utils.h" -#include +#include #include const static double ARROW_SCALE_RATIO = 6.0; @@ -90,7 +90,8 @@ namespace reach return marker; } - visualization_msgs::msg::InteractiveMarker makeInteractiveMarker(const reach_msgs::msg::ReachRecord &r, + visualization_msgs::msg::InteractiveMarker makeInteractiveMarker(const rclcpp::Node::SharedPtr &node, + const reach_msgs::msg::ReachRecord &r, const std::string &frame, const double scale) { @@ -104,7 +105,7 @@ namespace reach control.always_visible = true; // Visuals - auto visual = makeVisual(r, frame, scale); + auto visual = utils::makeVisual(node, r, frame, scale); control.markers.push_back(visual); m.controls.push_back(control); diff --git a/reach_demo/CMakeLists.txt b/reach_demo/CMakeLists.txt index 502b1902..b1bb7292 100644 --- a/reach_demo/CMakeLists.txt +++ b/reach_demo/CMakeLists.txt @@ -1,20 +1,21 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(reach_demo) -find_package(catkin REQUIRED) - -catkin_package() +find_package(ament_cmake REQUIRED) ############# ## Testing ## ############# -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - add_rostest(test/demo.test) -endif() +#if(CATKIN_ENABLE_TESTING) +# find_package(rostest REQUIRED) +# add_rostest(test/demo.test) +#endif() ############# ## Install ## ############# install(DIRECTORY config launch model results - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/reach_demo/config/params.yaml b/reach_demo/config/params.yaml index 8ccf8aed..89bf1ff6 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -1,40 +1,53 @@ -fixed_frame: "base_link" -object_frame: "reach_object" -results_directory: "$(find reach_demo)/results" -pcd_filename: "$(find reach_demo)/config/part.pcd" -get_avg_neighbor_count: false -compare_dbs: [] -visualize_results: true +robot_reach_study_node: + ros__parameters: + config_name: "demo_config" + fixed_frame: "base_link" + object_frame: "reach_object" + results_package: "reach_demo" + results_directory: "results" + pcd_package: "reach_demo" + pcd_filename_path: "config/part.pcd" + get_avg_neighbor_count: false + compare_dbs: [""] + visualize_results: true + planning_group: "manipulator" + run_initial_study_only: false -optimization: - radius: 0.2 - max_steps: 10 - step_improvement_threshold: 0.01 + optimization: + radius: 0.2 + max_steps: 10 + step_improvement_threshold: 0.01 -ik_solver_config: - name: "moveit_reach_plugins/ik/MoveItIKSolver" - distance_threshold: 0.0 - planning_group: "manipulator" - collision_mesh_filename: "package://reach_demo/config/part.ply" - collision_mesh_frame: "reach_object" - touch_links: [] - evaluation_plugin: - name: "reach_core/plugins/MultiplicativeFactory" - plugins: - - name: "moveit_reach_plugins/evaluation/ManipulabilityMoveIt" - planning_group: "manipulator" - - name: "moveit_reach_plugins/evaluation/DistancePenaltyMoveIt" - planning_group: "manipulator" - distance_threshold: 0.025 - exponent: 2 - collision_mesh_filename: "package://reach_demo/config/part.ply" - collision_mesh_frame: "reach_object" - touch_links: [] + ik_solver_config: + name: "moveit_reach_plugins/ik/MoveItIKSolver" + distance_threshold: 0.0 + planning_group: "manipulator" +# collision_mesh_package: "reach_demo" + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + touch_links: [""] + evaluation_plugin: + name: "reach_core/plugins/MultiplicativeFactory" + plugins: ["moveit_reach_plugins/evaluation/ManipulabilityMoveIt", "moveit_reach_plugins/evaluation/DistancePenaltyMoveIt"] + moveit_reach_plugins/evaluation/ManipulabilityMoveIt: + planning_group: "manipulator" + moveit_reach_plugins/evaluation/DistancePenaltyMoveIt: + planning_group: "manipulator" + distance_threshold: 0.025 + exponent: 2 +# collision_mesh_package: "reach_demo" + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + touch_links: [""] -display_config: - name: "moveit_reach_plugins/display/MoveItReachDisplay" - planning_group: "manipulator" - collision_mesh_filename: "package://reach_demo/config/part.ply" - collision_mesh_frame: "reach_object" - fixed_frame: "base_link" - marker_scale: 0.05 + display_config: + name: "moveit_reach_plugins/display/MoveItReachDisplay" + planning_group: "manipulator" +# collision_mesh_package: "reach_demo" + collision_mesh_package: "package://reach_demo/config/part.ply" + collision_mesh_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + fixed_frame: "base_link" + marker_scale: 0.05 diff --git a/reach_demo/launch/robot.launch.py b/reach_demo/launch/robot.launch.py new file mode 100644 index 00000000..19d3f5ac --- /dev/null +++ b/reach_demo/launch/robot.launch.py @@ -0,0 +1,177 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Lovro Ivanov +# fill param server and all necessary parameters without launching move group node + +import os + +import launch_ros.actions +import yaml +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", + default_value="true", + description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument("xacro_file", + default_value="reach_study.xacro", + description="Xacro file to parse.") + ) + + # General arguments + launch_rviz = LaunchConfiguration("launch_rviz") + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare("reach_demo"), "model", LaunchConfiguration("xacro_file")] + ), + ] + ) + + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("reach_demo"), + "model", + "reach_study.sdf", + ] + ), + ] + ) + + kinematics_yaml = load_yaml("reach_demo", "model/motoman_sia20d/config/kinematics.yaml") + + robot_description = {"robot_description": robot_description_content} + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} + + # # Planning Configuration + # ompl_planning_pipeline_config = { + # "move_group": { + # "planning_plugin": "ompl_interface/OMPLPlanner", + # "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + # "start_state_max_bounds_error": 0.1, + # } + # } + # ompl_planning_yaml = load_yaml( + # "gen3_robotiq_2f_85_move_it_config", "config/ompl_planning.yaml" + # ) + + # # Start the actual move_group node/action server + # move_group_node = Node( + # package="moveit_ros_move_group", + # executable="move_group", + # output="screen", + # parameters=[ + # robot_description, + # robot_description_semantic, + # robot_description_kinematics, + # ompl_planning_pipeline_config, + # trajectory_execution, + # moveit_controllers, + # planning_scene_monitor_parameters, + # ], + # ) + # + # # Warehouse mongodb server + # mongodb_server_node = Node( + # package="warehouse_ros_mongo", + # executable="mongo_wrapper_ros.py", + # parameters=[ + # {"warehouse_port": 33829}, + # {"warehouse_host": "localhost"}, + # {"warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection"}, + # ], + # output="screen", + # ) + # + # # rviz with moveit configuration + # rviz_config_file = PathJoinSubstitution( + # [FindPackageShare(moveit_config_package), "rviz", "moveit.rviz"] + # ) + # rviz_node = Node( + # package="rviz2", + # condition=IfCondition(launch_rviz), + # executable="rviz2", + # name="rviz2_moveit", + # output="log", + # arguments=["-d", rviz_config_file], + # parameters=[ + # robot_description, + # robot_description_semantic, + # ompl_planning_pipeline_config, + # robot_description_kinematics, + # ], + # ) + + # Static TF + # static_tf = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="log", + # arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], + # ) + + nodes_to_start = [ + # move_group_node, + # mongodb_server_node, + # rviz_node, + # static_tf, + ] + + # robot_description = {"robot_description": robot_description_content} + # robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + # robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml} + + return LaunchDescription(declared_arguments + + [launch_ros.actions.SetParameter(name="robot_description", value=robot_description)]) + + # return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/reach_demo/model/motoman_sia20d/config/controllers.yaml b/reach_demo/model/motoman_sia20d/config/controllers.yaml new file mode 100644 index 00000000..207f0ff2 --- /dev/null +++ b/reach_demo/model/motoman_sia20d/config/controllers.yaml @@ -0,0 +1,5 @@ +controller_manager: + ros__parameters: + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster diff --git a/reach_demo/model/motoman_sia20d/config/moveit_controllers.yaml b/reach_demo/model/motoman_sia20d/config/moveit_controllers.yaml new file mode 100644 index 00000000..a286580c --- /dev/null +++ b/reach_demo/model/motoman_sia20d/config/moveit_controllers.yaml @@ -0,0 +1,3 @@ +controller_names: + - joint_trajectory_controller + diff --git a/reach_demo/model/motoman_sia20d/config/ompl_planning.yaml b/reach_demo/model/motoman_sia20d/config/ompl_planning.yaml new file mode 100644 index 00000000..322fb71a --- /dev/null +++ b/reach_demo/model/motoman_sia20d/config/ompl_planning.yaml @@ -0,0 +1,204 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +manipulator: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(arm_joint_1,arm_joint_2) + longest_valid_segment_fraction: 0.005 +gripper: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +gantry_and_manipulator: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(arm_joint_1,arm_joint_2) + longest_valid_segment_fraction: 0.005 diff --git a/reach_demo/model/motoman_sia20d/motoman_sia20d.ros2_control.xacro b/reach_demo/model/motoman_sia20d/motoman_sia20d.ros2_control.xacro new file mode 100644 index 00000000..df478b9a --- /dev/null +++ b/reach_demo/model/motoman_sia20d/motoman_sia20d.ros2_control.xacro @@ -0,0 +1,64 @@ + + + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + + -3.1415 + 3.1415 + + + + + + -3.1415 + 3.1415 + + > + + + + -2.2689 + 2.2689 + + + + + + -3.1415 + 3.1415 + + + + + + -1.9198 + 1.9198 + + + + + + -2.9670 + 2.9670 + + + + + + -1.9198 + 1.9198 + + + + + + + diff --git a/reach_demo/model/motoman_sia20d/motoman_sia20d_macro.xacro b/reach_demo/model/motoman_sia20d/motoman_sia20d_macro.xacro index 30a9f92e..86ecddfb 100644 --- a/reach_demo/model/motoman_sia20d/motoman_sia20d_macro.xacro +++ b/reach_demo/model/motoman_sia20d/motoman_sia20d_macro.xacro @@ -3,8 +3,12 @@ Original: https://github.com/ros-industrial/motoman/blob/43be182c9cb4f62806e479b985965886c8603333/motoman_sia20d_support/urdf/sia20d_macro.xacro --> + - + + + + diff --git a/reach_demo/model/reach_study.srdf b/reach_demo/model/reach_study.srdf.xacro similarity index 57% rename from reach_demo/model/reach_study.srdf rename to reach_demo/model/reach_study.srdf.xacro index 55324e41..e3431c16 100644 --- a/reach_demo/model/reach_study.srdf +++ b/reach_demo/model/reach_study.srdf.xacro @@ -1,18 +1,8 @@ - - - - - - - + - @@ -22,9 +12,7 @@ - - diff --git a/reach_demo/package.xml b/reach_demo/package.xml index 02528e72..57a1b0a8 100644 --- a/reach_demo/package.xml +++ b/reach_demo/package.xml @@ -20,4 +20,8 @@ robot_state_publisher xacro ament_cmake_gtest + + + ament_cmake +