From 1181b80bfd7f617b497359519634237eea076e3b Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 20 Dec 2021 16:41:41 +0100 Subject: [PATCH 01/29] Reach study to rclcpp node and other changes. --- COLCON_IGNORE | 0 reach_core/CMakeLists.txt | 3 +- .../reach_core/plugins/evaluation_base.h | 2 +- .../reach_core/plugins/ik_solver_base.h | 2 +- .../reach_core/plugins/reach_display_base.h | 2 +- reach_core/include/reach_core/reach_study.h | 7 +- .../include/reach_core/study_parameters.h | 2 +- reach_core/src/core/reach_study.cpp | 15 +- reach_core/src/data_loader_node.cpp | 36 ++-- .../src/load_point_cloud_server_node.cpp | 201 +++++++++--------- .../plugins/impl/multiplicative_factory.cpp | 2 +- reach_core/src/robot_reach_study_node.cpp | 102 +++++---- 12 files changed, 207 insertions(+), 167 deletions(-) delete mode 100644 COLCON_IGNORE diff --git a/COLCON_IGNORE b/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index c38c7868..fc769f7c 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -7,7 +7,7 @@ find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(interactive_markers REQUIRED) find_package(moveit_core REQUIRED) -# find_package(PCL REQUIRED) +find_package(PCL REQUIRED) find_package(pcl_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) @@ -16,6 +16,7 @@ find_package(reach_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(fmt REQUIRED) find_package(OpenMP) if(OPENMP_FOUND) diff --git a/reach_core/include/reach_core/plugins/evaluation_base.h b/reach_core/include/reach_core/plugins/evaluation_base.h index 9ac96442..db593681 100644 --- a/reach_core/include/reach_core/plugins/evaluation_base.h +++ b/reach_core/include/reach_core/plugins/evaluation_base.h @@ -18,7 +18,7 @@ #include #include -#include +//#include namespace reach { 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..cfa12146 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -19,7 +19,7 @@ #include #include #include -#include +//#include #include namespace reach 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..de96b386 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -208,7 +208,7 @@ namespace reach 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_study.h b/reach_core/include/reach_core/reach_study.h index 9a07a1b2..acc053fd 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -24,6 +24,7 @@ // #include #include #include +#include namespace reach { @@ -33,14 +34,14 @@ namespace reach /** * @brief The ReachStudy class */ - class ReachStudy + class ReachStudy : public rclcpp::Node { public: /** * @brief ReachStudy * @param nh */ - ReachStudy(const rclcpp::Node::SharedPtr &node); + ReachStudy(const std::string & node_name, const rclcpp::NodeOptions & options); /** * @brief run @@ -62,8 +63,6 @@ namespace reach bool compareDatabases(); - ros::NodeHandle nh_; - StudyParameters sp_; pcl::PointCloud::Ptr cloud_; diff --git a/reach_core/include/reach_core/study_parameters.h b/reach_core/include/reach_core/study_parameters.h index 3c5e73d1..aaf4a551 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 { diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 24f927b8..b58bddab 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -23,9 +23,10 @@ #include #include #include -// #include #include +#include + const static std::string SAMPLE_MESH_SRV_TOPIC = "sample_mesh"; const static double SRV_TIMEOUT = 5.0; const static std::string INPUT_CLOUD_TOPIC = "input_cloud"; @@ -44,8 +45,12 @@ namespace reach 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) + ReachStudy::ReachStudy(const std::string & node_name, const rclcpp::NodeOptions & options) + : Node(node_name, options), + cloud_(new pcl::PointCloud()), + db_(new ReachDatabase()), + solver_loader_(PACKAGE, IK_BASE_CLASS), + display_loader_(PACKAGE, DISPLAY_BASE_CLASS) { } @@ -56,8 +61,8 @@ namespace reach 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) { diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index ee02d25b..15ce0f4f 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -14,31 +14,33 @@ * 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"; -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); @@ -59,7 +61,13 @@ int main(int argc, char **argv) return -1; } - std::string root_path = ros::package::getPath("reach_core") + "/" + RESULTS_FOLDER_NAME; + // Initialize ROS + rclcpp::init(argc, argv); + // create node + auto node = std::make_shared("data_loader_node"); + + + std::string root_path = std::string(ament_index_cpp::get_package_share_directory(("reach_core")) + "/" + RESULTS_FOLDER_NAME; if(argv[1]) { @@ -67,8 +75,8 @@ int main(int argc, char **argv) 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"; @@ -99,6 +107,8 @@ int main(int argc, char **argv) % res.avg_joint_distance; } } + // 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..c5200a87 100644 --- a/reach_core/src/load_point_cloud_server_node.cpp +++ b/reach_core/src/load_point_cloud_server_node.cpp @@ -13,111 +13,122 @@ * 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 -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; -} +constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesg"; + +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(std::string &node_name) : Node(node_name) { + + server_ = this->create_service(SAMPLE_MESH_SRV_TOPIC, [this](const LoadPCLReqSharedPtr req, + LoadPCLResSharedPtr res){ + + // 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 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(this->get_clock()); + tf2_ros::TransformListener listener(buffer); + Eigen::Isometry3d transform; + try { + 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); + } + 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::msg::PointCloud2 msg; + pcl::toROSMsg(transformed_cloud, res->cloud); + + res->success = true; + res->message = "Successfully loaded point cloud from '" + req->cloud_filename + "'"; + + return true; + }); + + } + + 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..8495a646 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -16,7 +16,7 @@ #include "reach_core/plugins/impl/multiplicative_factory.h" // #include #include -#include +//#include namespace reach { diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 3dfe0126..566aafcc 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -13,71 +13,85 @@ * 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; -} -bool getStudyParameters(ros::NodeHandle& nh, - reach::core::StudyParameters& sp) +class RobotReachStudyNode : public reach::core::ReachStudy { - 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; - } + explicit RobotReachStudyNode(std::string& node_name) : + reach::core::ReachStudy(node_name, + rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) + { - return true; -} + getStudyParameters(); + } + ~RobotReachStudyNode()=default; + +public: + bool getStudyParameters(){ + + // fetch parameteres + if (!this->get_parameter("config_name", sp_.config_name) || + !this->get_parameter("fixed_frame", sp_.fixed_frame) || + !this->get_parameter("results_directory", sp_.results_directory) || + !this->get_parameter("object_frame", sp_.object_frame) || + !this->get_parameter("pcd_filename", sp_.pcd_filename) || + !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", sp_.ik_solver_config) || + !this->get_parameter("display_config", sp_.display_config) ) { + return false; + }else{ + return true; + } + } + + bool run(){ + + return this->run(sp_); + } + +private: + + reach::core::StudyParameters sp_; + + + + +}; int main(int argc, char **argv) { - ros::init(argc, argv, "robot_reach_study_node"); - ros::NodeHandle pnh("~"), nh; - ros::AsyncSpinner spinner(1); - spinner.start(); + // Initialize ROS + rclcpp::init(argc, argv); + // create node + auto node = std::make_shared("robot_reach_study_node"); // Get the study parameters - reach::core::StudyParameters sp; - if(!getStudyParameters(pnh, sp)) + if(!node->getStudyParameters()) { return -1; } - // Initialize the reach study - reach::core::ReachStudy rs (nh); - // Run the reach study - if(!rs.run(sp)) + if(!node->run()) { - 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(); + + // spin + rclcpp::spin(node); + return 0; } From 781229c047f18e64f78997d693d556ca21fcc3f7 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 22 Dec 2021 13:25:19 +0100 Subject: [PATCH 02/29] Add shared ptr instead xmlrpc. Add basic launch file. --- .../display/moveit_reach_display.h | 2 +- .../evaluation/distance_penalty_moveit.h | 2 +- .../evaluation/joint_penalty_moveit.h | 2 +- .../evaluation/manipulability_moveit.h | 2 +- .../ik/discretized_moveit_ik_solver.h | 4 +- .../ik/moveit_ik_solver.h | 4 +- moveit_reach_plugins/package.xml | 1 + .../src/display/moveit_reach_display.cpp | 2 +- .../evaluation/distance_penalty_moveit.cpp | 2 +- .../src/evaluation/joint_penalty_moveit.cpp | 2 +- .../src/evaluation/manipulability_moveit.cpp | 2 +- .../src/ik/discretized_moveit_ik_solver.cpp | 6 +- .../src/ik/moveit_ik_solver.cpp | 10 +- reach_core/CMakeLists.txt | 117 ++++++++++-------- .../reach_core/plugins/evaluation_base.h | 5 +- .../reach_core/plugins/ik_solver_base.h | 7 +- .../plugins/impl/multiplicative_factory.h | 2 +- .../reach_core/plugins/reach_display_base.h | 48 +++---- .../include/reach_core/reach_database.h | 4 +- reach_core/include/reach_core/reach_study.h | 6 +- .../include/reach_core/study_parameters.h | 12 +- reach_core/launch/start.launch.py | 48 +++++++ reach_core/package.xml | 1 + reach_core/src/core/ik_helper.cpp | 2 +- reach_core/src/core/reach_study.cpp | 38 +++--- reach_core/src/core/reach_visualizer.cpp | 14 +-- .../plugins/impl/multiplicative_factory.cpp | 3 +- reach_core/src/robot_reach_study_node.cpp | 70 ++++++----- reach_demo/CMakeLists.txt | 19 +-- reach_demo/config/params.yaml | 81 ++++++------ reach_demo/package.xml | 4 + 31 files changed, 308 insertions(+), 214 deletions(-) create mode 100644 reach_core/launch/start.launch.py 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..54771eab 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 @@ -45,7 +45,7 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase MoveItReachDisplay(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; virtual void showEnvironment() override; 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..171f4e0f 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 @@ -46,7 +46,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) override; virtual double calculateScore(const std::map& pose) override; 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..0596480c 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 @@ -39,7 +39,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) 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..9cc5a6b8 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 @@ -39,7 +39,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) 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..dab869cb 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 @@ -29,9 +29,9 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver DiscretizedMoveItIKSolver(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) 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..6da9285b 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 @@ -48,9 +48,9 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase MoveItIKSolver(); - virtual bool initialize(XmlRpc::XmlRpcValue& config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) 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/package.xml b/moveit_reach_plugins/package.xml index b794479d..9f9501b0 100644 --- a/moveit_reach_plugins/package.xml +++ b/moveit_reach_plugins/package.xml @@ -27,6 +27,7 @@ + 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..acb4bc84 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -33,7 +33,7 @@ MoveItReachDisplay::MoveItReachDisplay() } -bool MoveItReachDisplay::initialize(XmlRpc::XmlRpcValue& config) +bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr &node) { if(!config.hasMember("planning_group") || !config.hasMember("collision_mesh_filename") || diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index fc7f875c..3a41515b 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -30,7 +30,7 @@ DistancePenaltyMoveIt::DistancePenaltyMoveIt() } -bool DistancePenaltyMoveIt::initialize(XmlRpc::XmlRpcValue& config) +bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr &node) { if(!config.hasMember("planning_group") || !config.hasMember("distance_threshold") || diff --git a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp index c387f03f..6a9baf58 100644 --- a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp @@ -30,7 +30,7 @@ JointPenaltyMoveIt::JointPenaltyMoveIt() } -bool JointPenaltyMoveIt::initialize(XmlRpc::XmlRpcValue& config) +bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr &node) { if(!config.hasMember("planning_group")) { diff --git a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp index 1ab7cd33..6eae44b1 100644 --- a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp @@ -30,7 +30,7 @@ ManipulabilityMoveIt::ManipulabilityMoveIt() } -bool ManipulabilityMoveIt::initialize(XmlRpc::XmlRpcValue& config) +bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr &node) { if(!config.hasMember("planning_group")) { 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..9ad17c44 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -43,9 +43,9 @@ DiscretizedMoveItIKSolver::DiscretizedMoveItIKSolver() } -bool DiscretizedMoveItIKSolver::initialize(XmlRpc::XmlRpcValue& config) +bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr &node) { - if(!MoveItIKSolver::initialize(config)) + if(!MoveItIKSolver::initialize(name, node)) { ROS_ERROR("Failed to initialize MoveItIKSolver plugin"); return false; @@ -87,7 +87,7 @@ 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); + std::optional score = MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution); if(score && (score.get() > best_score)) { best_score = *score; diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 62e9f4ee..cd71192a 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -36,7 +36,7 @@ MoveItIKSolver::MoveItIKSolver() } -bool MoveItIKSolver::initialize(XmlRpc::XmlRpcValue& config) +bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr &node) { if(!config.hasMember("planning_group") || !config.hasMember("distance_threshold") || @@ -123,7 +123,7 @@ bool MoveItIKSolver::initialize(XmlRpc::XmlRpcValue& config) 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) { @@ -144,7 +144,11 @@ boost::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& 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_ATTEMPTS, SOLUTION_TIMEOUT, std::bind(&MoveItIKSolver::isIKSolutionValid, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3))) { solution.clear(); state.copyJointGroupPositions(jmg_, solution); diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index fc769f7c..02fd74da 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -18,18 +18,18 @@ find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) find_package(fmt REQUIRED) -find_package(OpenMP) -if(OPENMP_FOUND) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") -endif() +#find_package(OpenMP) +#if(OPENMP_FOUND) +# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +#endif() set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs interactive_markers moveit_core - # pcl_ros + pcl_ros pcl_conversions pluginlib rclcpp @@ -39,33 +39,10 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS visualization_msgs ) -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 -# ) +#ament_export_libraries( +# src/${PROJECT_NAME} +# src/${PROJECT_NAME}_plugins +#) ########### ## BUILD ## @@ -73,8 +50,8 @@ ament_export_dependencies( include_directories( include + ${PCL_INCLUDE_DIRS} ) -include_directories(${PCL_INCLUDE_DIRS}) # Reach Study Library add_library(${PROJECT_NAME} @@ -88,22 +65,39 @@ 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}) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_plugins) +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) -# Plugins Library +## Plugins Library add_library(${PROJECT_NAME}_plugins src/plugins/impl/multiplicative_factory.cpp ) -# target_link_libraries(${PROJECT_NAME}_plugins -# ) -ament_target_dependencies(${PROJECT_NAME}_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories(${PROJECT_NAME}_plugins + PUBLIC + $ + $ +) +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 + $ + $ +) +#ament_target_dependencies(robot_reach_study_node ${PROJECT_NAME}) target_link_libraries(robot_reach_study_node ${PROJECT_NAME} ) @@ -115,7 +109,8 @@ ament_target_dependencies(robot_reach_study_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} @@ -138,11 +133,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 ## @@ -160,14 +155,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 DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include/${PROJECT_NAME} +) + install(FILES plugin_description.xml DESTINATION share/${PROJECT_NAME} ) + +## EXPORTS +ament_export_include_directories(include) +ament_export_libraries( + ${PROJECT_NAME} + ${PROJECT_NAME}_plugins +) +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + + +ament_package() diff --git a/reach_core/include/reach_core/plugins/evaluation_base.h b/reach_core/include/reach_core/plugins/evaluation_base.h index db593681..dc8d1708 100644 --- a/reach_core/include/reach_core/plugins/evaluation_base.h +++ b/reach_core/include/reach_core/plugins/evaluation_base.h @@ -18,7 +18,8 @@ #include #include -//#include + +#include namespace reach { @@ -43,7 +44,7 @@ namespace reach * @brief initialize * @param config */ - virtual bool initialize(XmlRpc::XmlRpcValue &config) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) = 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 cfa12146..d79d5edb 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -16,10 +16,9 @@ #ifndef REACH_CORE_PLUGINS_IK_IK_SOLVER_BASE_H #define REACH_CORE_PLUGINS_IK_IK_SOLVER_BASE_H -#include +#include #include #include -//#include #include namespace reach @@ -46,7 +45,7 @@ namespace reach * @param config * @return */ - virtual bool initialize(XmlRpc::XmlRpcValue &config) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) = 0; /** * @brief solveIKFromSeed attempts to find a valid IK solution for the given target pose starting from the input seed state. @@ -55,7 +54,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; 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..3685d4af 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,7 @@ namespace reach public: MultiplicativeFactory(); - virtual bool initialize(XmlRpc::XmlRpcValue &config) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) 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 de96b386..8ceeb53f 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -22,11 +22,10 @@ #include #include "reach_core/utils/visualization_utils.h" #include -#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"; +constexpr char INTERACTIVE_MARKER_TOPIC[] = "reach_int_markers"; +constexpr char REACH_DIFF_TOPIC[] = "reach_comparison"; +constexpr char MARKER_TOPIC[] = "reach_neighbors"; namespace reach { @@ -42,18 +41,21 @@ 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){ + + 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); + }; virtual void showEnvironment() = 0; @@ -61,14 +63,14 @@ namespace reach 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)); + server_->insert(std::move(marker)); menu_handler_.apply(server_, rec.id); } - server_.applyChanges(); + server_->applyChanges(); } void createMenuFunction(const std::string &menu_entry, @@ -79,12 +81,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); + server_->insert(marker); menu_handler_.apply(server_, rec.id); - server_.applyChanges(); + server_->applyChanges(); } else { @@ -101,7 +103,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; @@ -113,7 +115,7 @@ 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); + marker_pub_->publish(pt_marker); } } @@ -138,7 +140,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) @@ -189,7 +191,7 @@ namespace reach } } - diff_pub_.publish(marker_array); + diff_pub_->publish(marker_array); } protected: @@ -198,12 +200,10 @@ namespace reach 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_; diff --git a/reach_core/include/reach_core/reach_database.h b/reach_core/include/reach_core/reach_database.h index 8f188584..4e84efd0 100644 --- a/reach_core/include/reach_core/reach_database.h +++ b/reach_core/include/reach_core/reach_database.h @@ -18,9 +18,9 @@ #include "reach_core/study_parameters.h" #include -#include #include #include +#include namespace reach { @@ -90,7 +90,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 acc053fd..945148ff 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -21,7 +21,7 @@ #include #include #include -// #include + #include #include #include #include @@ -34,8 +34,8 @@ namespace reach /** * @brief The ReachStudy class */ - class ReachStudy : public rclcpp::Node - { + class ReachStudy : public rclcpp::Node { + public: /** * @brief ReachStudy diff --git a/reach_core/include/reach_core/study_parameters.h b/reach_core/include/reach_core/study_parameters.h index aaf4a551..b54205c2 100644 --- a/reach_core/include/reach_core/study_parameters.h +++ b/reach_core/include/reach_core/study_parameters.h @@ -49,12 +49,16 @@ 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_directory; - std::string pcd_filename; + std::string results_package; + std::string results_directory; + std::string pcd_package; + std::string pcd_filename_path; bool visualize_results; bool get_neighbors; std::vector compare_dbs; diff --git a/reach_core/launch/start.launch.py b/reach_core/launch/start.launch.py new file mode 100644 index 00000000..ae91f261 --- /dev/null +++ b/reach_core/launch/start.launch.py @@ -0,0 +1,48 @@ +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 + + +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" + ) + ) + + parameters_package = LaunchConfiguration("parameters_package") + parameters_filename = LaunchConfiguration("parameters_filename") + + study_parameters = PathJoinSubstitution( + [FindPackageShare(parameters_package), "config", parameters_filename] + ) + + robot_reach_study_node = Node( + package="reach_core", + executable="robot_reach_study_node", + name="robot_reach_study_node", + output="screen", + parameters=[study_parameters] + ) + + nodes_to_run = [robot_reach_study_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..af2f942a 100644 --- a/reach_core/package.xml +++ b/reach_core/package.xml @@ -31,6 +31,7 @@ + ament_cmake diff --git a/reach_core/src/core/ik_helper.cpp b/reach_core/src/core/ik_helper.cpp index 67ae9a57..c3246673 100644 --- a/reach_core/src/core/ik_helper.cpp +++ b/reach_core/src/core/ik_helper.cpp @@ -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_study.cpp b/reach_core/src/core/reach_study.cpp index b58bddab..9f775f95 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -23,15 +23,15 @@ #include #include #include -#include +#include #include -const static std::string SAMPLE_MESH_SRV_TOPIC = "sample_mesh"; +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 { @@ -41,9 +41,9 @@ 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"; + constexpr char PACKAGE[] = "reach_core"; + constexpr char IK_BASE_CLASS[] = "reach::plugins::IKSolverBase"; + constexpr char DISPLAY_BASE_CLASS[] = "reach::plugins::DisplayBase"; ReachStudy::ReachStudy(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), @@ -61,23 +61,23 @@ namespace reach try { - ik_solver_ = solver_loader_.createSharedInstance(sp_.ik_solver_config["name"]); - display_ = display_loader_.createSharedInstance(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 std::exception &ex) { - ROS_ERROR_STREAM(ex.getMessage()); + RCLCPP_ERROR(LOGGER, "Error while creating shared instances of ik solver and/or display: '%s'", ex.what()); return false; } catch (const pluginlib::PluginlibException &ex) { - ROS_ERROR_STREAM(ex.what()); + RCLCPP_ERROR(LOGGER, "Pluginlib exception thrown while creating shared instances of ik solver and/or display: '%s'", ex.what()); 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, this) || + !display_->initialize(sp_.display_config_name, this)) { return false; } @@ -85,7 +85,7 @@ namespace reach display_->showEnvironment(); // Create a directory to store results of study - if (!sp_.results_directory.empty() && boost::filesystem::exists(sp_.results_directory.c_str())) + if (!sp_.results_directory.empty() && std::filesystem::exists(sp_.results_directory.c_str())) { dir_ = sp_.results_directory + "/"; } @@ -97,10 +97,10 @@ namespace reach 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; diff --git a/reach_core/src/core/reach_visualizer.cpp b/reach_core/src/core/reach_visualizer.cpp index 9c8f1729..c9d7975e 100644 --- a/reach_core/src/core/reach_visualizer.cpp +++ b/reach_core/src/core/reach_visualizer.cpp @@ -39,11 +39,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, std::placeholders::_1); + CBType show_seed_cb = std::bind(&ReachVisualizer::showSeedCB, this, std::placeholders::_1); + CBType re_solve_ik_cb = std::bind(&ReachVisualizer::reSolveIKCB, this, std::placeholders::_1); + CBType neighbors_direct_cb = std::bind(&ReachVisualizer::reachNeighborsDirectCB, this, std::placeholders::_1); + CBType neighbors_recursive_cb = std::bind(&ReachVisualizer::reachNeighborsRecursiveCB, this, std::placeholders::_1); display_->createMenuFunction("Show Result", show_result_cb); display_->createMenuFunction("Show Seed Position", show_seed_cb); @@ -62,7 +62,7 @@ namespace reach void ReachVisualizer::reSolveIKCB(const visualization_msgs::msg::InteractiveMarkerFeedback *&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 +78,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) diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index 8495a646..593395f3 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -16,7 +16,6 @@ #include "reach_core/plugins/impl/multiplicative_factory.h" // #include #include -//#include namespace reach { @@ -36,7 +35,7 @@ namespace reach { } - bool MultiplicativeFactory::initialize(XmlRpc::XmlRpcValue &config) + bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr& node) { try { diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 566aafcc..9a6eda70 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -18,37 +18,55 @@ #include "reach_core/reach_study.h" #include "reach_core/study_parameters.h" +#include + class RobotReachStudyNode : public reach::core::ReachStudy { +public: explicit RobotReachStudyNode(std::string& node_name) : reach::core::ReachStudy(node_name, rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) { - + // get the study parameters getStudyParameters(); } - ~RobotReachStudyNode()=default; public: bool getStudyParameters(){ - // fetch parameteres - if (!this->get_parameter("config_name", sp_.config_name) || - !this->get_parameter("fixed_frame", sp_.fixed_frame) || - !this->get_parameter("results_directory", sp_.results_directory) || - !this->get_parameter("object_frame", sp_.object_frame) || - !this->get_parameter("pcd_filename", sp_.pcd_filename) || - !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", sp_.ik_solver_config) || - !this->get_parameter("display_config", sp_.display_config) ) { + // fetch parameteres !this->get_parameter("config_name", sp_.config_name) || + if (!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("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"), "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 ); + RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "compare_dbs: '%s'", sp_.compare_dbs[0].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() ); return true; } } @@ -62,32 +80,24 @@ class RobotReachStudyNode : public reach::core::ReachStudy reach::core::StudyParameters sp_; +}; -}; - int main(int argc, char **argv) { - // Initialize ROS rclcpp::init(argc, argv); // create node auto node = std::make_shared("robot_reach_study_node"); - // Get the study parameters - if(!node->getStudyParameters()) - { - return -1; - } // Run the reach study - if(!node->run()) - { - RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), "Unable to perform the reach study"); - return -1; - } - +// if(!node->run()) +// { +// RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), "Unable to perform the reach study"); +// return -1; +// } // spin rclcpp::spin(node); 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..ede7158c 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -1,40 +1,47 @@ -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: + 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 -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_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_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_filename_path: "config/part.ply" + collision_mesh_frame: "reach_object" + fixed_frame: "base_link" + marker_scale: 0.05 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 + From 1888aca505cfdafda399ef1bbb995f9d24987a38 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 23 Dec 2021 10:53:19 +0100 Subject: [PATCH 03/29] Successfully build reach_core. --- reach_core/CMakeLists.txt | 92 +++++++++++------- .../reach_core/plugins/ik_solver_base.h | 4 +- .../reach_core/plugins/reach_display_base.h | 19 ++-- .../include/reach_core/reach_database.h | 1 + reach_core/include/reach_core/reach_study.h | 19 +++- .../include/reach_core/reach_visualizer.h | 10 +- .../include/reach_core/study_parameters.h | 6 +- .../reach_core/utils/serialization_utils.h | 23 +++-- .../reach_core/utils/visualization_utils.h | 59 ++++++------ reach_core/package.xml | 6 +- reach_core/src/core/ik_helper.cpp | 4 +- reach_core/src/core/reach_database.cpp | 2 +- reach_core/src/core/reach_study.cpp | 94 ++++++++++--------- reach_core/src/core/reach_visualizer.cpp | 26 ++--- reach_core/src/data_loader_node.cpp | 2 +- .../src/load_point_cloud_server_node.cpp | 9 +- .../plugins/impl/multiplicative_factory.cpp | 20 ++-- reach_core/src/robot_reach_study_node.cpp | 60 +++++++----- reach_core/src/utils/visualization_utils.cpp | 7 +- 19 files changed, 268 insertions(+), 195 deletions(-) diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index 02fd74da..83e246d4 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -8,7 +8,8 @@ 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(Eigen3 REQUIRED) +#find_package(pcl_ros REQUIRED) find_package(pcl_conversions REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -29,7 +30,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs interactive_markers moveit_core - pcl_ros +# pcl_ros pcl_conversions pluginlib rclcpp @@ -39,11 +40,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS visualization_msgs ) -#ament_export_libraries( -# src/${PROJECT_NAME} -# src/${PROJECT_NAME}_plugins -#) - ########### ## BUILD ## ########### @@ -53,11 +49,52 @@ 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 +) + +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 @@ -70,24 +107,11 @@ target_include_directories(${PROJECT_NAME} $ $ ) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_plugins) +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_plugins ${PROJECT_NAME}_utils) 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}_plugins - PUBLIC - $ - $ -) -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 @@ -97,15 +121,16 @@ target_include_directories(robot_reach_study_node $ $ ) -#ament_target_dependencies(robot_reach_study_node ${PROJECT_NAME}) target_link_libraries(robot_reach_study_node ${PROJECT_NAME} + ${PROJECT_NAME}_utils + ${PROJECT_NAME}_plugins ) ament_target_dependencies(robot_reach_study_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -# Load Point Cloud Server Node +## Load Point Cloud Server Node add_executable(load_point_cloud_server_node src/load_point_cloud_server_node.cpp ) @@ -117,7 +142,7 @@ ament_target_dependencies(load_point_cloud_server_node ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -# Data Loader Node +## Data Loader Node add_executable(data_loader src/data_loader_node.cpp ) @@ -147,6 +172,7 @@ install( TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins + ${PROJECT_NAME}_utils robot_reach_study_node load_point_cloud_server_node data_loader @@ -155,11 +181,11 @@ install( RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install( - TARGETS - robot_reach_study_node - DESTINATION lib/${PROJECT_NAME} -) +#install( +# TARGETS +# robot_reach_study_node +# DESTINATION lib/${PROJECT_NAME} +#) install( DIRECTORY launch config @@ -179,10 +205,10 @@ 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/include/reach_core/plugins/ik_solver_base.h b/reach_core/include/reach_core/plugins/ik_solver_base.h index d79d5edb..d6220f4a 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -21,6 +21,8 @@ #include #include +#include + namespace reach { namespace plugins @@ -45,7 +47,7 @@ namespace reach * @param config * @return */ - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) = 0; /** * @brief solveIKFromSeed attempts to find a valid IK solution for the given target pose starting from the input seed state. 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 8ceeb53f..ba992d28 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -50,11 +50,14 @@ namespace reach marker_pub_.reset(); } - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node){ + bool initialize(std::string& name, rclcpp::Node::SharedPtr node){ + 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); + + return true; }; virtual void showEnvironment() = 0; @@ -66,9 +69,9 @@ namespace reach server_->clear(); for (const reach_msgs::msg::ReachRecord &rec : database.records) { - auto marker = utils::makeInteractiveMarker(rec, fixed_frame_, marker_scale_); + auto marker = utils::makeInteractiveMarker(node_, rec, fixed_frame_, marker_scale_); server_->insert(std::move(marker)); - menu_handler_.apply(server_, rec.id); + menu_handler_.apply(*server_, rec.id); } server_->applyChanges(); } @@ -83,9 +86,9 @@ namespace reach { if (server_->erase(rec.id)) { - auto marker = utils::makeInteractiveMarker(rec, fixed_frame_, marker_scale_); + auto marker = utils::makeInteractiveMarker(node_, rec, fixed_frame_, marker_scale_); server_->insert(marker); - menu_handler_.apply(server_, rec.id); + menu_handler_.apply(*server_, rec.id); server_->applyChanges(); } else @@ -114,7 +117,7 @@ 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_); + visualization_msgs::msg::Marker pt_marker = reach::utils::makeMarker(node_, pt_array, fixed_frame_, marker_scale_); marker_pub_->publish(pt_marker); } } @@ -186,7 +189,7 @@ 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); } } @@ -207,6 +210,8 @@ namespace reach std::shared_ptr> diff_pub_; std::shared_ptr> marker_pub_; + + std::shared_ptr node_; }; typedef std::shared_ptr DisplayBasePtr; diff --git a/reach_core/include/reach_core/reach_database.h b/reach_core/include/reach_core/reach_database.h index 4e84efd0..9e5dfd0b 100644 --- a/reach_core/include/reach_core/reach_database.h +++ b/reach_core/include/reach_core/reach_database.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace reach { diff --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index 945148ff..0e5d04c5 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -21,7 +21,7 @@ #include #include #include - #include +// #include #include #include #include @@ -34,14 +34,14 @@ namespace reach /** * @brief The ReachStudy class */ - class ReachStudy : public rclcpp::Node { + class ReachStudy { public: /** * @brief ReachStudy * @param nh */ - ReachStudy(const std::string & node_name, const rclcpp::NodeOptions & options); + ReachStudy(const rclcpp::Node::SharedPtr node); /** * @brief run @@ -50,10 +50,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 getReachObjectPointCloud(const rclcpp::Node::SharedPtr &node); + bool getReachObjectPointCloud(); void runInitialReachStudy(); @@ -84,6 +93,8 @@ namespace reach std::string results_dir_; sensor_msgs::msg::PointCloud2 cloud_msg_; + + std::shared_ptr node_; }; } // 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 b54205c2..eaf8574a 100644 --- a/reach_core/include/reach_core/study_parameters.h +++ b/reach_core/include/reach_core/study_parameters.h @@ -56,9 +56,9 @@ struct StudyParameters StudyOptimization optimization; std::string config_name; std::string results_package; - std::string results_directory; - std::string pcd_package; - std::string pcd_filename_path; + std::string results_directory; + std::string pcd_package; + std::string pcd_filename_path; bool visualize_results; bool get_neighbors; std::vector compare_dbs; diff --git a/reach_core/include/reach_core/utils/serialization_utils.h b/reach_core/include/reach_core/utils/serialization_utils.h index 3f8e099e..93a048c2 100644 --- a/reach_core/include/reach_core/utils/serialization_utils.h +++ b/reach_core/include/reach_core/utils/serialization_utils.h @@ -31,12 +31,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(); + auto ser_msg = new rclcpp::SerializedMessage(); + serializer.serialize_message(&msg, ser_msg); std::ofstream file(path.c_str(), std::ios::out | std::ios::binary); if (!file) @@ -45,7 +42,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,8 +51,6 @@ namespace reach bool fromFile(const std::string &path, T &msg) { - namespace ser = ros::serialization; - std::ifstream ifs(path.c_str(), std::ios::in | std::ios::binary); if (!ifs) { @@ -69,10 +64,14 @@ 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); + + auto ser_msg = new rclcpp::SerializedMessage(); + ser_msg->get_rcl_serialized_message().buffer = ibuffer.get(); + auto serializer = rclcpp::Serialization(); + serializer.deserialize_message(ser_msg, &msg); + 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/package.xml b/reach_core/package.xml index af2f942a..dd3a9b5b 100644 --- a/reach_core/package.xml +++ b/reach_core/package.xml @@ -19,15 +19,13 @@ interactive_markers moveit_core pcl_conversions - + pluginlib rclcpp reach_msgs tf2_ros tf2_eigen - visualization_msgs - - + visualization_msgs diff --git a/reach_core/src/core/ik_helper.cpp b/reach_core/src/core/ik_helper.cpp index c3246673..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) { diff --git a/reach_core/src/core/reach_database.cpp b/reach_core/src/core/reach_database.cpp index b489c576..eb1da9bc 100644 --- a/reach_core/src/core/reach_database.cpp +++ b/reach_core/src/core/reach_database.cpp @@ -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); diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 9f775f95..9b9d8fb8 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -21,12 +21,17 @@ #include #include -#include -#include +#include +#include #include +#include + #include +#include + + constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesh"; const static double SRV_TIMEOUT = 5.0; constexpr char INPUT_CLOUD_TOPIC[] = "input_cloud"; @@ -45,13 +50,14 @@ namespace reach constexpr char IK_BASE_CLASS[] = "reach::plugins::IKSolverBase"; constexpr char DISPLAY_BASE_CLASS[] = "reach::plugins::DisplayBase"; - ReachStudy::ReachStudy(const std::string & node_name, const rclcpp::NodeOptions & options) - : Node(node_name, options), + 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) { + } bool ReachStudy::initializeStudy() @@ -64,20 +70,20 @@ namespace reach ik_solver_ = solver_loader_.createSharedInstance(sp_.ik_solver_config_name); display_ = display_loader_.createSharedInstance(sp_.display_config_name); } - catch (const std::exception &ex) - { - RCLCPP_ERROR(LOGGER, "Error while creating shared instances of ik solver and/or display: '%s'", ex.what()); - return false; - } catch (const pluginlib::PluginlibException &ex) { RCLCPP_ERROR(LOGGER, "Pluginlib exception thrown while creating shared instances of ik solver and/or display: '%s'", ex.what()); return false; } + catch (const std::exception &ex) + { + RCLCPP_ERROR(LOGGER, "Error while creating shared instances of ik solver and/or display: '%s'", ex.what()); + return false; + } // Initialize the IK solver plugin and display plugin - if (!ik_solver_->initialize(sp_.ik_solver_config_name, this) || - !display_->initialize(sp_.display_config_name, this)) + if (!ik_solver_->initialize(sp_.ik_solver_config_name, node_) || + !display_->initialize(sp_.display_config_name, node_)) { return false; } @@ -85,14 +91,15 @@ namespace reach display_->showEnvironment(); // Create a directory to store results of study - if (!sp_.results_directory.empty() && std::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(); @@ -128,8 +135,8 @@ namespace reach // 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 @@ -208,7 +215,7 @@ 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"); } } } @@ -216,35 +223,38 @@ namespace reach 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); - - 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; - - 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) + auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC); + 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->wait_for_service(); + auto result = client->async_send_request(req); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR_STREAM(LOGGER, srv.response.message); - return false; - } + if (!result.get()->success) + { + RCLCPP_ERROR_STREAM(LOGGER, result.get()->message); + return false; + } - cloud_msg_ = srv.response.cloud; - pcl::fromROSMsg(cloud_msg_, *cloud_); + cloud_msg_ = result.get()->cloud; + 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; + } else { + RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client->get_service_name() << "'"); + return false; + } - return true; } void ReachStudy::runInitialReachStudy() @@ -273,7 +283,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; diff --git a/reach_core/src/core/reach_visualizer.cpp b/reach_core/src/core/reach_visualizer.cpp index c9d7975e..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 = std::bind(&ReachVisualizer::showResultCB, this, std::placeholders::_1); - CBType show_seed_cb = std::bind(&ReachVisualizer::showSeedCB, this, std::placeholders::_1); - CBType re_solve_ik_cb = std::bind(&ReachVisualizer::reSolveIKCB, this, std::placeholders::_1); - CBType neighbors_direct_cb = std::bind(&ReachVisualizer::reachNeighborsDirectCB, this, std::placeholders::_1); - CBType neighbors_recursive_cb = std::bind(&ReachVisualizer::reachNeighborsRecursiveCB, this, std::placeholders::_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,7 +64,7 @@ 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) { std::optional lookup = db_->get(fb->marker_name); if (lookup) @@ -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 15ce0f4f..5d9ecda2 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -67,7 +67,7 @@ int main(int argc, char **argv) auto node = std::make_shared("data_loader_node"); - std::string root_path = std::string(ament_index_cpp::get_package_share_directory(("reach_core")) + "/" + RESULTS_FOLDER_NAME; + std::string root_path = std::string(ament_index_cpp::get_package_share_directory(("reach_core"))) + "/" + RESULTS_FOLDER_NAME; if(argv[1]) { diff --git a/reach_core/src/load_point_cloud_server_node.cpp b/reach_core/src/load_point_cloud_server_node.cpp index c5200a87..8c8da1c1 100644 --- a/reach_core/src/load_point_cloud_server_node.cpp +++ b/reach_core/src/load_point_cloud_server_node.cpp @@ -19,13 +19,14 @@ #include #include #include -#include +//#include +#include #include #include #include -#include +#include -constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesg"; +constexpr char SAMPLE_MESH_SRV_TOPIC[] = "sample_mesh"; using LoadPCLSrv = reach_msgs::srv::LoadPointCloud; using LoadPCLReq = reach_msgs::srv::LoadPointCloud_Request; @@ -36,7 +37,7 @@ using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; class PointCloudServerNode : public rclcpp::Node { public: - explicit PointCloudServerNode(std::string &node_name) : Node(node_name) { + explicit PointCloudServerNode(const std::string &node_name) : Node(node_name) { server_ = this->create_service(SAMPLE_MESH_SRV_TOPIC, [this](const LoadPCLReqSharedPtr req, LoadPCLResSharedPtr res){ diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index 593395f3..bca40f83 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -14,7 +14,6 @@ * limitations under the License. */ #include "reach_core/plugins/impl/multiplicative_factory.h" -// #include #include namespace reach @@ -27,8 +26,8 @@ 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) @@ -39,19 +38,20 @@ namespace reach { 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); EvaluationBasePtr plugin; try { - plugin = class_loader_.createSharedInstance(name); + plugin = class_loader_.createSharedInstance(plugin_name); } catch (const pluginlib::ClassLoaderException &ex) { @@ -59,7 +59,7 @@ namespace reach continue; } - if (!plugin->initialize(plugin_config)) + if (!plugin->initialize(name, node)) { RCLCPP_WARN_STREAM(LOGGER, "Plugin '" << name << "' failed to be initialized; excluding it from the list"); continue; @@ -68,9 +68,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 9a6eda70..dc42c1d8 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -21,22 +21,21 @@ #include -class RobotReachStudyNode : public reach::core::ReachStudy +class RobotReachStudyNode : public rclcpp::Node { public: - explicit RobotReachStudyNode(std::string& node_name) : - reach::core::ReachStudy(node_name, - rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) + explicit RobotReachStudyNode(const std::string& node_name) + : Node(node_name, rclcpp::NodeOptions().allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true)) { - // get the study parameters - getStudyParameters(); + } public: - bool getStudyParameters(){ + bool getStudyParameters(reach::core::StudyParameters& sp){ - // fetch parameteres !this->get_parameter("config_name", sp_.config_name) || - if (!this->get_parameter("fixed_frame", sp_.fixed_frame) || + // 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) || @@ -53,6 +52,7 @@ class RobotReachStudyNode : public reach::core::ReachStudy 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() ); @@ -63,17 +63,18 @@ class RobotReachStudyNode : public reach::core::ReachStudy 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 ); - RCLCPP_INFO(rclcpp::get_logger("robot_reach_study_node"), "compare_dbs: '%s'", sp_.compare_dbs[0].c_str() ); + 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() ); - return true; - } - } - bool run(){ + // set params + sp = sp_; - return this->run(sp_); + return true; + } } private: @@ -91,17 +92,30 @@ int main(int argc, char **argv) // create node auto node = std::make_shared("robot_reach_study_node"); + // get the study parameters + reach::core::StudyParameters sp; + if (!node->getStudyParameters(sp)){ - // Run the reach study -// if(!node->run()) -// { -// RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), "Unable to perform the reach study"); -// return -1; -// } + return -1; + } - // spin - rclcpp::spin(node); + std::thread t1( [node]{ + // spin + rclcpp::spin(node); + }); + // Initialize the reach study + reach::core::ReachStudy rs (node); + + // Run the reach study + if(!rs.run(sp) || !rclcpp::ok()) + { + RCLCPP_ERROR(rclcpp::get_logger("robot_reach_study_node"), "Unable to perform the reach study"); + return -1; + } + + 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); From 72eeaa45fcc4b861c74b1a4babd5814736a7c05f Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 23 Dec 2021 10:53:51 +0100 Subject: [PATCH 04/29] Adapt demo parameters. --- reach_demo/config/params.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/reach_demo/config/params.yaml b/reach_demo/config/params.yaml index ede7158c..4db1ec52 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -1,5 +1,6 @@ robot_reach_study_node: ros__parameters: + config_name: "demo_config" fixed_frame: "base_link" object_frame: "reach_object" results_package: "reach_demo" From 05699499d16b3a0d5bb5b2f88161c541fea246b0 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 23 Dec 2021 11:02:48 +0100 Subject: [PATCH 05/29] Install include directories correctly. --- reach_core/CMakeLists.txt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index 83e246d4..78712c72 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -192,8 +192,12 @@ install( DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION include/${PROJECT_NAME} +#install(DIRECTORY include/${PROJECT_NAME} +# DESTINATION include/${PROJECT_NAME} +#) + +install(DIRECTORY include/ + DESTINATION include ) install(FILES plugin_description.xml From ff6289a9e126533d13c884802a938109f3a5ae3e Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 23 Dec 2021 17:56:09 +0100 Subject: [PATCH 06/29] Successfully build the whole ros2 reach package. --- moveit_reach_plugins/CMakeLists.txt | 130 +++++++++--------- .../display/moveit_reach_display.h | 16 ++- .../evaluation/distance_penalty_moveit.h | 11 +- .../evaluation/joint_penalty_moveit.h | 6 +- .../evaluation/manipulability_moveit.h | 6 +- .../ik/discretized_moveit_ik_solver.h | 3 +- .../ik/moveit_ik_solver.h | 15 +- .../include/moveit_reach_plugins/utils.h | 26 ++-- moveit_reach_plugins/package.xml | 3 +- .../src/display/moveit_reach_display.cpp | 67 ++++----- .../evaluation/distance_penalty_moveit.cpp | 57 +++----- .../src/evaluation/joint_penalty_moveit.cpp | 32 ++--- .../src/evaluation/manipulability_moveit.cpp | 29 ++-- .../src/ik/discretized_moveit_ik_solver.cpp | 30 ++-- .../src/ik/moveit_ik_solver.cpp | 86 +++++------- moveit_reach_plugins/src/utils.cpp | 61 ++++---- reach_core/CMakeLists.txt | 10 +- .../reach_core/plugins/evaluation_base.h | 2 +- .../plugins/impl/multiplicative_factory.h | 2 +- .../reach_core/plugins/reach_display_base.h | 7 +- .../plugins/impl/multiplicative_factory.cpp | 2 +- 21 files changed, 295 insertions(+), 306 deletions(-) diff --git a/moveit_reach_plugins/CMakeLists.txt b/moveit_reach_plugins/CMakeLists.txt index 8792da95..182b9f44 100644 --- a/moveit_reach_plugins/CMakeLists.txt +++ b/moveit_reach_plugins/CMakeLists.txt @@ -1,41 +1,21 @@ -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_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 +23,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,13 +49,14 @@ 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} ) # MoveIt IK Solver Plugin @@ -78,39 +64,47 @@ 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 + reach_core + pluginlib + rclcpp + tf2_eigen +) # 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 ) # 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 +116,17 @@ 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} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include/${PROJECT_NAME} ) install(FILES plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + DESTINATION share/${PROJECT_NAME} ) + +ament_package() \ No newline at end of file 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 54771eab..ec8dd91e 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,6 +17,9 @@ #define MOVEIT_REACH_PLUGINS_MOVEIT_REACH_DISPLAY_H #include +#include + +#include namespace moveit { @@ -36,6 +39,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,7 +52,7 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase MoveItReachDisplay(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; virtual void showEnvironment() override; @@ -59,13 +66,14 @@ 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_; + rclcpp::Node::SharedPtr n_; - 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 171f4e0f..6553063c 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,7 +17,7 @@ #define MOVEIT_REACH_PLUGINS_EVALUATION_DISTANCE_PENALTY_MOVEIT_H #include -#include +#include namespace moveit { @@ -37,6 +37,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 +50,7 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase DistancePenaltyMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; virtual double calculateScore(const std::map& pose) override; @@ -62,7 +66,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 0596480c..0296aa21 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 @@ -30,6 +30,10 @@ class JointModelGroup; namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.JointPenaltyMoveIt"); + } namespace evaluation { @@ -39,7 +43,7 @@ class JointPenaltyMoveIt : public reach::plugins::EvaluationBase JointPenaltyMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) 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 9cc5a6b8..688ee5f8 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 @@ -30,6 +30,10 @@ class JointModelGroup; namespace moveit_reach_plugins { + namespace + { + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.ManipulabilityMoveIt"); + } namespace evaluation { @@ -39,7 +43,7 @@ class ManipulabilityMoveIt : public reach::plugins::EvaluationBase ManipulabilityMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) 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 dab869cb..b8da0122 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,7 +30,7 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver DiscretizedMoveItIKSolver(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map& seed, 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 6da9285b..703e7493 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,7 +18,7 @@ #include #include -#include +#include namespace moveit { @@ -39,6 +39,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,7 +52,7 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase MoveItIKSolver(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map &seed, @@ -74,9 +78,12 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase 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::string collision_mesh_frame_; std::vector touch_links_; }; 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 9f9501b0..84e574b8 100644 --- a/moveit_reach_plugins/package.xml +++ b/moveit_reach_plugins/package.xml @@ -14,7 +14,7 @@ https://github.com/ros-industrial/reach/ ament_cmake - eigen_conversions + tf2_eigen interactive_markers moveit_core moveit_msgs @@ -23,7 +23,6 @@ reach_core reach_msgs visualization_msgs - xmlrpcpp diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index acb4bc84..e02c1f19 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -16,9 +16,8 @@ #include "moveit_reach_plugins/display/moveit_reach_display.h" #include "moveit_reach_plugins/utils.h" #include -#include #include -#include +#include const static std::string PLANNING_SCENE_TOPIC = "planning_scene_display"; @@ -33,44 +32,37 @@ MoveItReachDisplay::MoveItReachDisplay() } -bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr &node) +bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node) { - if(!config.hasMember("planning_group") || - !config.hasMember("collision_mesh_filename") || - !config.hasMember("collision_mesh_frame") || - !config.hasMember("fixed_frame") || - !config.hasMember("marker_scale")) - { - ROS_ERROR("MoveIt IK Solver Plugin is missing one or more configuration parameters"); - return false; - } + reach::plugins::DisplayBase::initialize(name, node); - 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) + n_ = node; + + 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 + "marker_scale", marker_scale_)) { - ROS_ERROR_STREAM(ex.getMessage()); + RCLCPP_ERROR(LOGGER, "MoveIt IK Solver Plugin is missing one or more configuration parameters"); return false; } - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); + + model_ = moveit::planning_interface::getSharedRobotModel(node, "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; } @@ -79,30 +71,31 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr & // 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); + std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, 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; } - 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) @@ -111,21 +104,21 @@ void MoveItReachDisplay::updateRobotPose(const std::map& po 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"); } } } // 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 3a41515b..344e075f 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,49 +30,33 @@ DistancePenaltyMoveIt::DistancePenaltyMoveIt() } -bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr &node) +bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node) { - 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]); - } - } - catch(const XmlRpc::XmlRpcException& ex) - { - ROS_ERROR_STREAM(ex.getMessage()); - return false; - } - - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); + model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); 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; } @@ -81,16 +65,17 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt // 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); + std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, 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 +92,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 +107,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 6a9baf58..643ddf44 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,29 @@ JointPenaltyMoveIt::JointPenaltyMoveIt() } -bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr &node) +bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node) { - 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::getSharedRobotModel(node, "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("Failed to initialize joint model group pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize joint model group pointer"); return false; } @@ -78,7 +70,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 +93,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 +107,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 6eae44b1..bb63289e 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,36 +29,28 @@ ManipulabilityMoveIt::ManipulabilityMoveIt() } -bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr &node) +bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node) { - 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 - { - planning_group = std::string(config["planning_group"]); - } - catch(const XmlRpc::XmlRpcException& ex) + std::string param_prefix ("ik_solver_config.evaluation_plugin.moveit_reach_plugins/evaluation/ManipulabilityMoveIt."); + if(!node->get_parameter(param_prefix+ "planning_group", planning_group)) { - 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"); + model_ = moveit::planning_interface::getSharedRobotModel(node, "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("Failed to initialize joint model group pointer"); + RCLCPP_ERROR(LOGGER, "Failed to initialize joint model group pointer"); return false; } @@ -75,7 +66,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 +90,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 9ad17c44..41f2881d 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,35 +40,38 @@ DiscretizedMoveItIKSolver::DiscretizedMoveItIKSolver() } -bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr &node) +bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) { if(!MoveItIKSolver::initialize(name, node)) { - 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) { @@ -88,9 +88,9 @@ boost::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen:: std::vector tmp_solution; std::optional score = MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution); - if(score && (score.get() > best_score)) + if(score.has_value() && (score.value() > best_score)) { - best_score = *score; + best_score = score.value(); best_solution = std::move(tmp_solution); } else @@ -102,7 +102,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 +113,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 cd71192a..1299d0ed 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,64 +34,55 @@ MoveItIKSolver::MoveItIKSolver() } -bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr &node) +bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) { - 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")) + 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.touch_links", touch_links_) || + !node->get_parameter("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]); - } 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; - } - - model_ = moveit::planning_interface::getSharedRobotModel("robot_description"); + if(!eval_->initialize(evaluation_plugin_name_, node)) + { + 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::getSharedRobotModel(node, "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; } @@ -102,16 +91,17 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr &node // 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); + std::string mesh_path_tmp = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + collision_mesh_filename_path_; + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(mesh_path_tmp, 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,7 +109,7 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr &node 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; } @@ -134,17 +124,17 @@ std::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& t 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, std::bind(&MoveItIKSolver::isIKSolutionValid, + if(state.setFromIK(jmg_, target, SOLUTION_TIMEOUT, std::bind(&MoveItIKSolver::isIKSolutionValid, this, std::placeholders::_1, std::placeholders::_2, @@ -189,5 +179,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..2c037185 100644 --- a/moveit_reach_plugins/src/utils.cpp +++ b/moveit_reach_plugins/src/utils.cpp @@ -18,33 +18,38 @@ #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; + 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); 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; @@ -53,24 +58,25 @@ namespace moveit_reach_plugins 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 +87,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 +123,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 +148,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 +182,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 +194,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 78712c72..50c3fe2e 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -17,7 +17,7 @@ find_package(reach_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) -find_package(fmt REQUIRED) +#find_package(fmt REQUIRED) #find_package(OpenMP) #if(OPENMP_FOUND) @@ -138,7 +138,7 @@ target_link_libraries(load_point_cloud_server_node ${PROJECT_NAME} ) ament_target_dependencies(load_point_cloud_server_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} +# ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) @@ -192,9 +192,9 @@ install( DESTINATION share/${PROJECT_NAME} ) -#install(DIRECTORY include/${PROJECT_NAME} -# DESTINATION include/${PROJECT_NAME} -#) +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include/${PROJECT_NAME} +) install(DIRECTORY include/ DESTINATION include diff --git a/reach_core/include/reach_core/plugins/evaluation_base.h b/reach_core/include/reach_core/plugins/evaluation_base.h index dc8d1708..7a283a4d 100644 --- a/reach_core/include/reach_core/plugins/evaluation_base.h +++ b/reach_core/include/reach_core/plugins/evaluation_base.h @@ -44,7 +44,7 @@ namespace reach * @brief initialize * @param config */ - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) = 0; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) = 0; /** * @brief calculateScore 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 3685d4af..f1e00b19 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,7 @@ namespace reach public: MultiplicativeFactory(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr &node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) 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 ba992d28..9a562c22 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include "reach_core/utils/visualization_utils.h" #include @@ -50,7 +50,7 @@ namespace reach marker_pub_.reset(); } - bool initialize(std::string& name, rclcpp::Node::SharedPtr node){ + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node){ node_ = node; server_ = std::make_shared(INTERACTIVE_MARKER_TOPIC, node); @@ -196,6 +196,8 @@ namespace reach diff_pub_->publish(marker_array); } + public: + std::shared_ptr node_; protected: std::string fixed_frame_ = "base_frame"; @@ -211,7 +213,6 @@ namespace reach std::shared_ptr> marker_pub_; - std::shared_ptr node_; }; typedef std::shared_ptr DisplayBasePtr; diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index bca40f83..543aa092 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -34,7 +34,7 @@ namespace reach { } - bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr& node) + bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr node) { try { From be541e17bf69f4f0390e50a40256d18f5230530c Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 28 Dec 2021 13:18:15 +0100 Subject: [PATCH 07/29] Address pluginlib issues. --- moveit_reach_plugins/CMakeLists.txt | 27 ++- .../display_plugin_description.xml | 9 + .../eval_plugin_description.xml | 31 ++++ .../ik_plugin_description.xml | 16 ++ moveit_reach_plugins/package.xml | 1 - reach_core/CMakeLists.txt | 2 +- reach_core/launch/setup.launch.py | 63 +++++++ reach_core/plugin_description.xml | 2 +- .../{config => rviz}/reach_study_config.rviz | 0 reach_demo/launch/robot.launch.py | 169 ++++++++++++++++++ 10 files changed, 312 insertions(+), 8 deletions(-) create mode 100644 moveit_reach_plugins/display_plugin_description.xml create mode 100644 moveit_reach_plugins/eval_plugin_description.xml create mode 100644 moveit_reach_plugins/ik_plugin_description.xml create mode 100644 reach_core/launch/setup.launch.py rename reach_core/{config => rviz}/reach_study_config.rviz (100%) create mode 100644 reach_demo/launch/robot.launch.py diff --git a/moveit_reach_plugins/CMakeLists.txt b/moveit_reach_plugins/CMakeLists.txt index 182b9f44..b406e137 100644 --- a/moveit_reach_plugins/CMakeLists.txt +++ b/moveit_reach_plugins/CMakeLists.txt @@ -116,17 +116,34 @@ install( evaluation_plugins ik_solver_plugins reach_display_plugins + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION include/${PROJECT_NAME} +install(DIRECTORY include/ + DESTINATION include/ ) -install(FILES plugin_description.xml - DESTINATION share/${PROJECT_NAME} -) +#install(FILES display_plugin_description.xml eval_plugin_description.xml ik_plugin_description.xml +# DESTINATION share/${PROJECT_NAME} +#) +#pluginlib_export_plugin_description_file(moveit_reach_plugins display_plugin_description.xml) +#pluginlib_export_plugin_description_file(moveit_reach_plugins eval_plugin_description.xml) +pluginlib_export_plugin_description_file(reach_core ik_plugin_description.xml) + +ament_export_libraries( + ${PROJECT_NAME}_utils + evaluation_plugins + ik_solver_plugins + reach_display_plugins +) +ament_export_targets( + export_${PROJECT_NAME} +) +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/eval_plugin_description.xml b/moveit_reach_plugins/eval_plugin_description.xml new file mode 100644 index 00000000..fe663ad1 --- /dev/null +++ b/moveit_reach_plugins/eval_plugin_description.xml @@ -0,0 +1,31 @@ + + + + + + A pose evalution plugin which returns a score (range [0, inf)) that is the manipulability of robot at the input pose calculated from the robot's Jacobian. + Higher scores indicate higher robot dexterity at a given pose. + + + + + + + A pose evaluation plugin which returns a score that is the product of each joint's distance from the middle of its joint range, according to the following equation: + score[i] = ((joint[i] - joint_min[i])*(joint_max[i] - joint[i])) / (joint_max[i] - joint_min[i])^2 + The score for each joint will lie in the range [0, 0.25]; therefore, the score for an entire robot will lie in the range n*[0, 0.25], where n is the number of joints + in the robot. Higher scores indicate poses which are closer to the nominal middle of travel of each joint. This plugin should be used if it is desirable for kinematic + solutions to be near the middle of all joint ranges. + + + + + + + A pose evaluation plugin which returns a score (range [0, inf]) based on the robot's distance to the nearest collision, with respect to an input threshold and according + to the following equation: score = (closest_distance_to_collision - input_threshold)^input_exponent. + If the nearest distance to collision is less than the threshold, the score will exponentially approach zero; if the nearest distance collision is greater than the threshold, + the score will exponentially approach infinity. This plugin should be used if it is desirable to prefer solutions that are at least the threshold distance away from collision. + + + 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/package.xml b/moveit_reach_plugins/package.xml index 84e574b8..72ab32e9 100644 --- a/moveit_reach_plugins/package.xml +++ b/moveit_reach_plugins/package.xml @@ -25,7 +25,6 @@ visualization_msgs - ament_cmake diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index 50c3fe2e..a1f514ef 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -188,7 +188,7 @@ install( #) install( - DIRECTORY launch config + DIRECTORY launch config rviz DESTINATION share/${PROJECT_NAME} ) diff --git a/reach_core/launch/setup.launch.py b/reach_core/launch/setup.launch.py new file mode 100644 index 00000000..392863d7 --- /dev/null +++ b/reach_core/launch/setup.launch.py @@ -0,0 +1,63 @@ +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" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "rviz_config_package", + description="Package where to find rviz file under /rviz subfolder.", + default_value="reach_core" + ) + ) + + visualize_results = LaunchConfiguration("visualize_results") + rviz_config_package = LaunchConfiguration("rviz_config_package") + + # rviz configuration + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(rviz_config_package), + "rviz", + "reach_study_config.rviz"] + ) + + rviz_node = Node( + package="rviz2", + condition=IfCondition(visualize_results), + executable="rviz2", + name="rviz2_moveit", + output="log", + # arguments=["-d", rviz_config_file], + parameters=[], + ) + + 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, + rviz_node] + + return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file 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/config/reach_study_config.rviz b/reach_core/rviz/reach_study_config.rviz similarity index 100% rename from reach_core/config/reach_study_config.rviz rename to reach_core/rviz/reach_study_config.rviz diff --git a/reach_demo/launch/robot.launch.py b/reach_demo/launch/robot.launch.py new file mode 100644 index 00000000..75b3e1cc --- /dev/null +++ b/reach_demo/launch/robot.launch.py @@ -0,0 +1,169 @@ +# 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 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(LaunchConfiguration("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, + ] + + return LaunchDescription(declared_arguments + nodes_to_start) From 2730506e26597bb13f41b6c1e83eafbd594f4055 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 30 Dec 2021 12:54:49 +0100 Subject: [PATCH 08/29] From static to dynamic libs. --- moveit_reach_plugins/CMakeLists.txt | 23 ++++++++++------------- moveit_reach_plugins/package.xml | 2 +- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/moveit_reach_plugins/CMakeLists.txt b/moveit_reach_plugins/CMakeLists.txt index b406e137..03f930ed 100644 --- a/moveit_reach_plugins/CMakeLists.txt +++ b/moveit_reach_plugins/CMakeLists.txt @@ -3,6 +3,9 @@ project(moveit_reach_plugins) add_compile_options(-std=c++11) +# 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) @@ -58,6 +61,7 @@ target_link_libraries(evaluation_plugins ${PROJECT_NAME}_utils ${PCL_LIBRARIES} ) +pluginlib_export_plugin_description_file(reach_core eval_plugin_description.xml) # MoveIt IK Solver Plugin add_library(ik_solver_plugins @@ -78,6 +82,8 @@ ament_target_dependencies(ik_solver_plugins 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 @@ -93,6 +99,7 @@ target_link_libraries(reach_display_plugins ${PCL_LIBRARIES} ik_solver_plugins ) +pluginlib_export_plugin_description_file(reach_core display_plugin_description.xml) # IK Plugin Test #add_executable(ik_plugin_test @@ -119,31 +126,21 @@ install( EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ ) -#install(FILES display_plugin_description.xml eval_plugin_description.xml ik_plugin_description.xml -# DESTINATION share/${PROJECT_NAME} -#) - -#pluginlib_export_plugin_description_file(moveit_reach_plugins display_plugin_description.xml) -#pluginlib_export_plugin_description_file(moveit_reach_plugins eval_plugin_description.xml) -pluginlib_export_plugin_description_file(reach_core ik_plugin_description.xml) - ament_export_libraries( ${PROJECT_NAME}_utils evaluation_plugins ik_solver_plugins reach_display_plugins ) -ament_export_targets( - export_${PROJECT_NAME} -) + ament_export_include_directories( - include + include ) ament_package() \ No newline at end of file diff --git a/moveit_reach_plugins/package.xml b/moveit_reach_plugins/package.xml index 72ab32e9..adb692d2 100644 --- a/moveit_reach_plugins/package.xml +++ b/moveit_reach_plugins/package.xml @@ -13,7 +13,7 @@ https://github.com/ros-industrial/reach/issues https://github.com/ros-industrial/reach/ - ament_cmake + ament_cmake_ros tf2_eigen interactive_markers moveit_core From 77898db680c759c1fb15eab0702a6d0d6a099974 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 30 Dec 2021 16:49:52 +0100 Subject: [PATCH 09/29] Add hardcoded paths for test. --- moveit_reach_plugins/CMakeLists.txt | 11 ++++ .../ik/moveit_ik_solver.h | 10 +-- moveit_reach_plugins/plugin_description.xml | 58 ---------------- .../src/display/moveit_reach_display.cpp | 8 ++- .../evaluation/distance_penalty_moveit.cpp | 11 +++- .../src/evaluation/joint_penalty_moveit.cpp | 4 +- .../src/evaluation/manipulability_moveit.cpp | 7 +- .../src/ik/moveit_ik_solver.cpp | 15 +++-- moveit_reach_plugins/src/utils.cpp | 7 ++ reach_core/CMakeLists.txt | 12 ++-- reach_core/launch/start.launch.py | 66 ++++++++++++++++++- reach_core/package.xml | 2 +- .../plugins/impl/multiplicative_factory.cpp | 1 + reach_core/src/robot_reach_study_node.cpp | 11 +++- reach_demo/launch/robot.launch.py | 12 +++- ...each_study.srdf => reach_study.srdf.xacro} | 14 +--- 16 files changed, 144 insertions(+), 105 deletions(-) delete mode 100644 moveit_reach_plugins/plugin_description.xml rename reach_demo/model/{reach_study.srdf => reach_study.srdf.xacro} (57%) diff --git a/moveit_reach_plugins/CMakeLists.txt b/moveit_reach_plugins/CMakeLists.txt index 03f930ed..f157a855 100644 --- a/moveit_reach_plugins/CMakeLists.txt +++ b/moveit_reach_plugins/CMakeLists.txt @@ -61,6 +61,15 @@ target_link_libraries(evaluation_plugins ${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 @@ -77,6 +86,8 @@ target_link_libraries(ik_solver_plugins ${PROJECT_NAME}_utils ) ament_target_dependencies(ik_solver_plugins + moveit_ros_planning_interface + geometric_shapes reach_core pluginlib rclcpp 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 703e7493..55345047 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 @@ -67,24 +67,18 @@ 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_package_; std::string collision_mesh_filename_path_; std::string evaluation_plugin_name_; - - - std::string collision_mesh_frame_; - + std::string collision_mesh_frame_; std::vector touch_links_; }; diff --git a/moveit_reach_plugins/plugin_description.xml b/moveit_reach_plugins/plugin_description.xml deleted file mode 100644 index 42f66746..00000000 --- a/moveit_reach_plugins/plugin_description.xml +++ /dev/null @@ -1,58 +0,0 @@ - - - - - - A pose evalution plugin which returns a score (range [0, inf)) that is the manipulability of robot at the input pose calculated from the robot's Jacobian. - Higher scores indicate higher robot dexterity at a given pose. - - - - - - - A pose evaluation plugin which returns a score that is the product of each joint's distance from the middle of its joint range, according to the following equation: - score[i] = ((joint[i] - joint_min[i])*(joint_max[i] - joint[i])) / (joint_max[i] - joint_min[i])^2 - The score for each joint will lie in the range [0, 0.25]; therefore, the score for an entire robot will lie in the range n*[0, 0.25], where n is the number of joints - in the robot. Higher scores indicate poses which are closer to the nominal middle of travel of each joint. This plugin should be used if it is desirable for kinematic - solutions to be near the middle of all joint ranges. - - - - - - - A pose evaluation plugin which returns a score (range [0, inf]) based on the robot's distance to the nearest collision, with respect to an input threshold and according - to the following equation: score = (closest_distance_to_collision - input_threshold)^input_exponent. - If the nearest distance to collision is less than the threshold, the score will exponentially approach zero; if the nearest distance collision is greater than the threshold, - the score will exponentially approach infinity. This plugin should be used if it is desirable to prefer solutions that are at least the threshold distance away from collision. - - - - - - - - - - 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/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index e02c1f19..3d27e013 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -77,8 +77,12 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n // Add the collision object to the planning scene const std::string object_name = "reach_object"; - std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; - moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, collision_mesh_frame_, object_name); +// std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; +// const std::string tmp_mesh_filename = "/home/lovro/workspace/ros2_kortex_ws/src/reach/reach_demo/config/part.ply"; + const std::string tmp_mesh_filename = "package://reach_demo/config/part.ply"; + + + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index 344e075f..52051ae4 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -46,7 +46,9 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt return false; } - model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); +// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); + model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + if(!model_) { RCLCPP_ERROR(LOGGER, "Failed to load robot model"); @@ -71,8 +73,11 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt // Add the collision mesh object to the planning scene const std::string object_name = "reach_object"; - std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; - moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, collision_mesh_frame_, object_name); +// std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; +// const std::string tmp_mesh_filename = "/home/lovro/workspace/ros2_kortex_ws/src/reach/reach_demo/config/part.ply"; + const std::string tmp_mesh_filename = "package://reach_demo/config/part.ply"; + + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); diff --git a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp index 643ddf44..df092e9b 100644 --- a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp @@ -40,7 +40,9 @@ bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr n return false; } - model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); +// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); + model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + if(!model_) { diff --git a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp index bb63289e..041daad6 100644 --- a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp @@ -40,8 +40,10 @@ bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr return false; } - model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); - if(!model_) + RCLCPP_INFO(LOGGER, "Creating shared robot model in the node '%s' using parameter robot_description", node->get_name()); +// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); + model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + if(!model_) { RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); return false; @@ -54,6 +56,7 @@ bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr return false; } + RCLCPP_INFO(LOGGER, "moveit_reach_plugins/evaluation/ManipulabilityMoveIt initialized successfully."); return true; } diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 1299d0ed..ca157b63 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -42,14 +42,14 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) !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.touch_links", touch_links_) || - !node->get_parameter("evaluation_plugin.name", evaluation_plugin_name_)) + !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_)) { RCLCPP_ERROR(LOGGER, "MoveIt IK Solver Plugin is missing one or more configuration parameters"); return false; } - try { eval_ = class_loader_.createSharedInstance(evaluation_plugin_name_); @@ -72,7 +72,10 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) return false; } - model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); + RCLCPP_INFO(LOGGER, "Initializing robot shared model"); +// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); + model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + if(!model_) { RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); @@ -97,7 +100,9 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) // Add the collision object to the planning scene const std::string object_name = "reach_object"; - std::string mesh_path_tmp = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + collision_mesh_filename_path_; +// std::string mesh_path_tmp = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; +// const std::string mesh_path_tmp = "/home/lovro/workspace/ros2_kortex_ws/src/reach/reach_demo/config/part.ply"; + const std::string mesh_path_tmp = "package://reach_demo/config/part.ply"; moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(mesh_path_tmp, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { diff --git a/moveit_reach_plugins/src/utils.cpp b/moveit_reach_plugins/src/utils.cpp index 2c037185..da33c0e2 100644 --- a/moveit_reach_plugins/src/utils.cpp +++ b/moveit_reach_plugins/src/utils.cpp @@ -39,11 +39,17 @@ namespace moveit_reach_plugins const std::string &object_name) { // Create a CollisionObject message for the reach object + 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.operation = obj.ADD; @@ -55,6 +61,7 @@ namespace moveit_reach_plugins pose.orientation.w = 1.0; obj.mesh_poses.push_back(pose); + return obj; } diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index a1f514ef..31597cc8 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -4,6 +4,7 @@ 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) @@ -71,6 +72,7 @@ ament_target_dependencies(${PROJECT_NAME}_plugins visualization_msgs reach_msgs ) +pluginlib_export_plugin_description_file(reach_core plugin_description.xml) add_library(${PROJECT_NAME}_utils # Utilities @@ -192,18 +194,14 @@ install( DESTINATION share/${PROJECT_NAME} ) -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION include/${PROJECT_NAME} -) +#install(DIRECTORY include/${PROJECT_NAME} +# DESTINATION include/${PROJECT_NAME} +#) install(DIRECTORY include/ DESTINATION include ) -install(FILES plugin_description.xml - DESTINATION share/${PROJECT_NAME} -) - ## EXPORTS ament_export_include_directories(include) ament_export_libraries( diff --git a/reach_core/launch/start.launch.py b/reach_core/launch/start.launch.py index ae91f261..ddc364c4 100644 --- a/reach_core/launch/start.launch.py +++ b/reach_core/launch/start.launch.py @@ -7,7 +7,27 @@ 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(): @@ -27,20 +47,64 @@ def generate_launch_description(): 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.") + ) parameters_package = LaunchConfiguration("parameters_package") parameters_filename = LaunchConfiguration("parameters_filename") + moveit_config_file = LaunchConfiguration("moveit_config_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] + ), + ] + ) + 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] + parameters=[ + study_parameters, + robot_description, + robot_description_semantic, + robot_description_kinematics + ], ) nodes_to_run = [robot_reach_study_node] diff --git a/reach_core/package.xml b/reach_core/package.xml index dd3a9b5b..0ca8bc51 100644 --- a/reach_core/package.xml +++ b/reach_core/package.xml @@ -13,7 +13,7 @@ https://github.com/ros-industrial/reach/issues https://github.com/ros-industrial/reach/ - ament_cmake + ament_cmake_ros geometry_msgs interactive_markers diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index 543aa092..005f421d 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -47,6 +47,7 @@ namespace reach { 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 diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index dc42c1d8..2c2f4ce6 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -89,9 +89,15 @@ int main(int argc, char **argv) { // Initialize ROS rclcpp::init(argc, argv); + + rclcpp::executors::MultiThreadedExecutor executor; + + // 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)){ @@ -99,9 +105,10 @@ int main(int argc, char **argv) return -1; } - std::thread t1( [node]{ + std::thread t1( [&executor]{ // spin - rclcpp::spin(node); +// rclcpp::spin(node); + executor.spin(); }); // Initialize the reach study diff --git a/reach_demo/launch/robot.launch.py b/reach_demo/launch/robot.launch.py index 75b3e1cc..19d3f5ac 100644 --- a/reach_demo/launch/robot.launch.py +++ b/reach_demo/launch/robot.launch.py @@ -17,6 +17,7 @@ import os +import launch_ros.actions import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription @@ -65,7 +66,7 @@ def generate_launch_description(): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare(LaunchConfiguration("reach_demo")), "model", LaunchConfiguration("xacro_file")] + [FindPackageShare("reach_demo"), "model", LaunchConfiguration("xacro_file")] ), ] ) @@ -166,4 +167,11 @@ def generate_launch_description(): # static_tf, ] - return LaunchDescription(declared_arguments + nodes_to_start) + # 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/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 @@ - - From 5e80577cb71328a24cf7ebcc4b1305081144851b Mon Sep 17 00:00:00 2001 From: Lovro Date: Sat, 8 Jan 2022 14:08:05 +0100 Subject: [PATCH 10/29] Port up to publishing input cloud. --- .../src/display/moveit_reach_display.cpp | 8 +- .../reach_core/plugins/reach_display_base.h | 2 +- reach_core/launch/start.launch.py | 10 ++- reach_core/src/core/reach_study.cpp | 89 ++++++++++++++----- .../src/load_point_cloud_server_node.cpp | 37 ++++++-- reach_core/src/robot_reach_study_node.cpp | 10 +++ 6 files changed, 125 insertions(+), 31 deletions(-) diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index 3d27e013..ded09b62 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -34,6 +34,7 @@ MoveItReachDisplay::MoveItReachDisplay() bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node) { + RCLCPP_INFO(LOGGER, "Initializing MoveItReachDisplay!"); reach::plugins::DisplayBase::initialize(name, node); n_ = node; @@ -45,6 +46,7 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n !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_)) { RCLCPP_ERROR(LOGGER, "MoveIt IK Solver Plugin is missing one or more configuration parameters"); @@ -52,8 +54,10 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n } - model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); - if(!model_) +// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); + model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + + if(!model_) { RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); return false; 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 9a562c22..ed357d67 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -56,7 +56,7 @@ namespace reach 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; }; diff --git a/reach_core/launch/start.launch.py b/reach_core/launch/start.launch.py index ddc364c4..c363c778 100644 --- a/reach_core/launch/start.launch.py +++ b/reach_core/launch/start.launch.py @@ -107,6 +107,14 @@ def generate_launch_description(): ], ) - nodes_to_run = [robot_reach_study_node] + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + nodes_to_run = [robot_reach_study_node, + robot_state_publisher_node] return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 9b9d8fb8..73c0a5be 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -28,7 +28,7 @@ #include #include - +#include #include @@ -85,7 +85,10 @@ namespace reach if (!ik_solver_->initialize(sp_.ik_solver_config_name, node_) || !display_->initialize(sp_.display_config_name, node_)) { + RCLCPP_ERROR(LOGGER, "Could not initialized both display and ik solver plugins!"); return false; + }else { + RCLCPP_INFO(LOGGER, "IK and display solver successfully initialized!"); } display_->showEnvironment(); @@ -123,6 +126,8 @@ namespace reach { RCLCPP_ERROR(LOGGER, "Failed to initialize the reach study"); return false; + }else { + RCLCPP_INFO(LOGGER, "Reach study initialized!"); } // Get the reach object point cloud @@ -130,6 +135,8 @@ namespace reach { RCLCPP_ERROR(LOGGER, "Unable to obtain reach object point cloud"); return false; + }else { + RCLCPP_INFO(LOGGER, "Reach object point cloud obtained successfully!"); } // Show the reach object collision object and reach object point cloud @@ -137,6 +144,8 @@ namespace reach { rclcpp::Publisher::SharedPtr pub = node_->create_publisher(INPUT_CLOUD_TOPIC, 1); pub->publish(cloud_msg_); + }else { + RCLCPP_INFO(LOGGER, "Not visualizing results!"); } // Create markers @@ -226,34 +235,72 @@ namespace reach 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 req = std::make_shared(); + auto callback_group_input_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); +// auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC); + auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC, rmw_qos_profile_services_default, callback_group_input_); +// get_input_client_ = node_->create_client("GetInput", rmw_qos_profile_services_default, callback_group_input_); + + 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; + RCLCPP_INFO(LOGGER, "Waiting for service '%s'.", SAMPLE_MESH_SRV_TOPIC); client->wait_for_service(); - auto result = client->async_send_request(req); - // Wait for the result. - if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) - { - if (!result.get()->success) - { - RCLCPP_ERROR_STREAM(LOGGER, result.get()->message); - return false; - } +// auto result = client->async_send_request(req); + bool success_tmp = false; - cloud_msg_ = result.get()->cloud; - pcl::fromROSMsg(cloud_msg_, *cloud_); - cloud_msg_.header.frame_id = sp_.fixed_frame; - cloud_msg_.header.stamp = node_->now(); + auto inner_client_callback = [&,this](rclcpp::Client::SharedFuture inner_future) + { + RCLCPP_INFO(LOGGER, "Inner service callback started"); + 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()); + RCLCPP_INFO(LOGGER, "Inner service callback finished"); + }; + auto inner_future_result = client->async_send_request(req, inner_client_callback); - return true; - } else { - RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client->get_service_name() << "'"); - return false; - } + // quick fix to wait for inner callback to finish + //TODO(livanov93) Add visible flag within the inner callback + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + + if (success_tmp){ + pcl::fromROSMsg(cloud_msg_, *cloud_); + + cloud_msg_.header.frame_id = sp_.fixed_frame; + cloud_msg_.header.stamp = node_->now(); + + return true; + + } else { + RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client->get_service_name() + << "'"); + return false; + } + + // Wait for the result. + { +// if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) == +// rclcpp::FutureReturnCode::SUCCESS) { +// if (!result.get()->success) { +// RCLCPP_ERROR_STREAM(LOGGER, result.get()->message); +// return false; +// } +// +// cloud_msg_ = result.get()->cloud; +// pcl::fromROSMsg(cloud_msg_, *cloud_); +// +// cloud_msg_.header.frame_id = sp_.fixed_frame; +// cloud_msg_.header.stamp = node_->now(); +// +// return true; +// } else { +// RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client->get_service_name() +// << "'"); +// return false; +// } + } } diff --git a/reach_core/src/load_point_cloud_server_node.cpp b/reach_core/src/load_point_cloud_server_node.cpp index 8c8da1c1..5cbadcbc 100644 --- a/reach_core/src/load_point_cloud_server_node.cpp +++ b/reach_core/src/load_point_cloud_server_node.cpp @@ -42,20 +42,22 @@ using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; 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 true; + 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 true; + return false; } if (!hasNormals(cloud_msg)) { @@ -73,16 +75,32 @@ using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; 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; - return true; + 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; @@ -91,13 +109,20 @@ using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; // Convert point cloud to message for output sensor_msgs::msg::PointCloud2 msg; pcl::toROSMsg(transformed_cloud, res->cloud); + for(size_t i = 0; i < res->cloud.data.size(); ++i){ + if (res->cloud.data[i]!= 0.0) { + RCLCPP_INFO(this->get_logger(), "%d-th data = %f ", i, res->cloud.data[i]); + } + } res->success = true; res->message = "Successfully loaded point cloud from '" + req->cloud_filename + "'"; - return true; - }); + RCLCPP_INFO(this->get_logger(), "Service callback finished!"); + return true; + }); + server_->get_service_name(); } private: diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 2c2f4ce6..1618e09a 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -109,6 +109,16 @@ int main(int argc, char **argv) // spin // rclcpp::spin(node); executor.spin(); + +// rclcpp::WallRate loop_rate(100); +// while (rclcpp::ok()) { +// +//// executor.spin_once(); +// rclcpp::spin_some(node); +// loop_rate.sleep(); +// } + + }); // Initialize the reach study From 98b9b6129abb44c9b5b825309769feb0d91d86a4 Mon Sep 17 00:00:00 2001 From: Lovro Date: Sat, 8 Jan 2022 20:37:52 +0100 Subject: [PATCH 11/29] Initial start of demo successfull. --- .../evaluation/distance_penalty_moveit.cpp | 4 ++ .../src/ik/moveit_ik_solver.cpp | 4 ++ .../reach_core/plugins/reach_display_base.h | 2 +- .../reach_core/utils/serialization_utils.h | 16 +++-- reach_core/launch/start.launch.py | 33 +++++++++- reach_core/src/core/ik_helper.cpp | 3 + reach_core/src/core/reach_database.cpp | 4 +- reach_core/src/core/reach_study.cpp | 14 ++-- reach_core/src/robot_reach_study_node.cpp | 7 +- .../motoman_sia20d/config/controllers.yaml | 5 ++ .../motoman_sia20d.ros2_control.xacro | 64 +++++++++++++++++++ .../motoman_sia20d/motoman_sia20d_macro.xacro | 6 +- 12 files changed, 149 insertions(+), 13 deletions(-) create mode 100644 reach_demo/model/motoman_sia20d/config/controllers.yaml create mode 100644 reach_demo/model/motoman_sia20d/motoman_sia20d.ros2_control.xacro diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index 52051ae4..30ed342a 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -46,6 +46,10 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt return false; } + if (std::find(touch_links_.begin(), touch_links_.end(), "") != touch_links_.end()){ + touch_links_.clear(); + } + // model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index ca157b63..65e51cf7 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -50,6 +50,10 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) return false; } + if (std::find(touch_links_.begin(), touch_links_.end(), "") != touch_links_.end()){ + touch_links_.clear(); + } + try { eval_ = class_loader_.createSharedInstance(evaluation_plugin_name_); 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 ed357d67..7782da19 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -200,7 +200,7 @@ namespace reach std::shared_ptr node_; protected: - std::string fixed_frame_ = "base_frame"; + std::string fixed_frame_ = "base"; double marker_scale_ = 1.0; diff --git a/reach_core/include/reach_core/utils/serialization_utils.h b/reach_core/include/reach_core/utils/serialization_utils.h index 93a048c2..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 { @@ -32,8 +33,8 @@ namespace reach const T &msg) { auto serializer = rclcpp::Serialization(); - auto ser_msg = new rclcpp::SerializedMessage(); - serializer.serialize_message(&msg, ser_msg); + rclcpp::SerializedMessage ser_msg; + serializer.serialize_message(&msg, &ser_msg); std::ofstream file(path.c_str(), std::ios::out | std::ios::binary); if (!file) @@ -42,7 +43,7 @@ namespace reach } else { - file.write((char *)ser_msg->get_rcl_serialized_message().buffer, ser_msg->capacity()); + file.write((char *)ser_msg.get_rcl_serialized_message().buffer, ser_msg.capacity()); return file.good(); } } @@ -51,9 +52,11 @@ namespace reach bool fromFile(const std::string &path, T &msg) { +// 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; } @@ -67,11 +70,16 @@ namespace reach std::shared_ptr ibuffer(new uint8_t[file_size]); ifs.read((char *)ibuffer.get(), file_size); +// 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/launch/start.launch.py b/reach_core/launch/start.launch.py index c363c778..daf969c2 100644 --- a/reach_core/launch/start.launch.py +++ b/reach_core/launch/start.launch.py @@ -62,10 +62,18 @@ def generate_launch_description(): 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] @@ -89,6 +97,11 @@ def generate_launch_description(): ), ] ) + + 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} @@ -107,6 +120,22 @@ def generate_launch_description(): ], ) + 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", @@ -115,6 +144,8 @@ def generate_launch_description(): ) nodes_to_run = [robot_reach_study_node, - robot_state_publisher_node] + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner] return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/src/core/ik_helper.cpp b/reach_core/src/core/ik_helper.cpp index 5cd3c754..a59b5ac0 100644 --- a/reach_core/src/core/ik_helper.cpp +++ b/reach_core/src/core/ik_helper.cpp @@ -184,9 +184,12 @@ namespace reach tf2::fromMsg(neighbors[i].goal, target); // Use current point's IK solution as seed +// RCLCPP_INFO(rclcpp::get_logger("ik_helper"), "Before solve..."); std::optional score = solver->solveIKFromSeed(target, current_pose_map, new_pose); +// RCLCPP_INFO(rclcpp::get_logger("ik_helper"), "After solve..."); if (score) { +// RCLCPP_INFO(rclcpp::get_logger("ik_helper"), "Score exists..."); // Calculate the joint distance between the seed and new goal states for (std::size_t j = 0; j < current_pose.size(); ++j) { diff --git a/reach_core/src/core/reach_database.cpp b/reach_core/src/core/reach_database.cpp index eb1da9bc..641eb33b 100644 --- a/reach_core/src/core/reach_database.cpp +++ b/reach_core/src/core/reach_database.cpp @@ -89,12 +89,14 @@ namespace reach bool ReachDatabase::load(const std::string &filename) { + RCLCPP_INFO(LOGGER, "ReachDatabase::load from '%s'", filename.c_str()); 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; } - + RCLCPP_INFO(LOGGER, "ReachDatabase::load ==> loaded from file successfully!"); std::lock_guard lock{mutex_}; for (const auto &r : msg.records) diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 73c0a5be..3af4b62a 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -150,10 +150,12 @@ namespace reach // Create markers visualizer_.reset(new ReachVisualizer(db_, ik_solver_, display_, sp_.optimization.radius)); +// RCLCPP_INFO(LOGGER, "Visualizer created!"); // 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)) { @@ -176,6 +178,7 @@ namespace reach visualizer_->update(); } + RCLCPP_INFO(LOGGER, "Creating search tree!"); // Create an efficient search tree for doing nearest neighbors search search_tree_.reset(new SearchTree(flann::KDTreeSingleIndexParams(1, true))); @@ -212,6 +215,7 @@ namespace reach // Perform the calculation if it hasn't already been done if (db_->getStudyResults().avg_num_neighbors == 0.0f) { + RCLCPP_INFO(LOGGER, "Performing average neighbour calculation.") ; getAverageNeighborsCount(); } } @@ -426,17 +430,19 @@ namespace reach current_counter = previous_pct = neighbor_count = 0; std::atomic total_joint_distance; const int total = db_->size(); - -// Iterate + int calc = 0; + // Iterate #pragma parallel for for (auto it = db_->begin(); it != db_->end(); ++it) { +// RCLCPP_INFO(LOGGER, "Calculation no %d", calc++); reach_msgs::msg::ReachRecord msg = it->second; if (msg.reached) { NeighborReachResult result; - reachNeighborsRecursive(db_, msg, ik_solver_, sp_.optimization.radius, result); //, search_tree_); - +// RCLCPP_INFO(LOGGER, "Before recursion..."); + reachNeighborsRecursive(db_, msg, ik_solver_, sp_.optimization.radius, result, search_tree_); +// RCLCPP_INFO(LOGGER, "After recursion..."); neighbor_count += static_cast(result.reached_pts.size() - 1); total_joint_distance = total_joint_distance + result.joint_distance; } diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 1618e09a..3de6601d 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -31,7 +31,8 @@ class RobotReachStudyNode : public rclcpp::Node } public: - bool getStudyParameters(reach::core::StudyParameters& sp){ + bool + getStudyParameters(reach::core::StudyParameters& sp){ // fetch parameteres if (!this->get_parameter("config_name", sp_.config_name) || @@ -70,6 +71,10 @@ class RobotReachStudyNode : public rclcpp::Node 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_; 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/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 --> + - + + + + From 00cf151795bf6d8dab75c413ddfbc867bb1c634d Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 11 Jan 2022 08:51:48 -0700 Subject: [PATCH 12/29] Move rviz to start launch file. --- .../src/ik/moveit_ik_solver.cpp | 4 ++-- reach_core/launch/setup.launch.py | 3 ++- reach_core/launch/start.launch.py | 18 +++++++++++++++++- .../src/load_point_cloud_server_node.cpp | 10 +++++----- 4 files changed, 26 insertions(+), 9 deletions(-) diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 65e51cf7..599ec17e 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -42,8 +42,8 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) !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.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_)) { RCLCPP_ERROR(LOGGER, "MoveIt IK Solver Plugin is missing one or more configuration parameters"); diff --git a/reach_core/launch/setup.launch.py b/reach_core/launch/setup.launch.py index 392863d7..e6be2810 100644 --- a/reach_core/launch/setup.launch.py +++ b/reach_core/launch/setup.launch.py @@ -58,6 +58,7 @@ def generate_launch_description(): ) nodes_to_run = [load_point_cloud_server_node, - rviz_node] + # rviz_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 index daf969c2..42333d9b 100644 --- a/reach_core/launch/start.launch.py +++ b/reach_core/launch/start.launch.py @@ -143,9 +143,25 @@ def generate_launch_description(): parameters=[robot_description], ) + 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] + joint_state_broadcaster_spawner, + rviz_node] return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/src/load_point_cloud_server_node.cpp b/reach_core/src/load_point_cloud_server_node.cpp index 5cbadcbc..638ca2bf 100644 --- a/reach_core/src/load_point_cloud_server_node.cpp +++ b/reach_core/src/load_point_cloud_server_node.cpp @@ -109,11 +109,11 @@ using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; // Convert point cloud to message for output sensor_msgs::msg::PointCloud2 msg; pcl::toROSMsg(transformed_cloud, res->cloud); - for(size_t i = 0; i < res->cloud.data.size(); ++i){ - if (res->cloud.data[i]!= 0.0) { - RCLCPP_INFO(this->get_logger(), "%d-th data = %f ", i, res->cloud.data[i]); - } - } +// for(size_t i = 0; i < res->cloud.data.size(); ++i){ +// if (res->cloud.data[i]!= 0.0) { +// RCLCPP_INFO(this->get_logger(), "%d-th data = %f ", i, res->cloud.data[i]); +// } +// } res->success = true; res->message = "Successfully loaded point cloud from '" + req->cloud_filename + "'"; From 83aa8c84927023de1aad6e428292f9edf53ebe0f Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 11 Jan 2022 12:12:39 -0700 Subject: [PATCH 13/29] Run move group with the reach analysis. --- .../src/display/moveit_reach_display.cpp | 20 +- .../evaluation/distance_penalty_moveit.cpp | 6 +- .../src/evaluation/joint_penalty_moveit.cpp | 2 - .../src/evaluation/manipulability_moveit.cpp | 3 - reach_core/launch/start.launch.py | 55 ++++- reach_demo/config/params.yaml | 6 +- .../config/moveit_controllers.yaml | 3 + .../motoman_sia20d/config/ompl_planning.yaml | 204 ++++++++++++++++++ 8 files changed, 277 insertions(+), 22 deletions(-) create mode 100644 reach_demo/model/motoman_sia20d/config/moveit_controllers.yaml create mode 100644 reach_demo/model/motoman_sia20d/config/ompl_planning.yaml diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index ded09b62..7b156e02 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -53,9 +53,7 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n return false; } - -// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); - model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); if(!model_) { @@ -81,16 +79,14 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n // Add the collision object to the planning scene const std::string object_name = "reach_object"; -// std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; -// const std::string tmp_mesh_filename = "/home/lovro/workspace/ros2_kortex_ws/src/reach/reach_demo/config/part.ply"; - const std::string tmp_mesh_filename = "package://reach_demo/config/part.ply"; - + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(collision_mesh_package_, collision_mesh_frame_, object_name); - moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_mesh_filename, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { 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_ = node_->create_publisher(PLANNING_SCENE_TOPIC, 1); @@ -101,13 +97,19 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n void MoveItReachDisplay::showEnvironment() { - moveit_msgs::msg::PlanningScene scene_msg; + while (scene_pub_->get_subscription_count() < 1) + { + RCLCPP_INFO(LOGGER, "No subscribers. Not showing environment..."); + rclcpp::sleep_for(std::chrono::milliseconds(500)); + } + moveit_msgs::msg::PlanningScene scene_msg; scene_->getPlanningSceneMsg(scene_msg); scene_pub_->publish(scene_msg); } void MoveItReachDisplay::updateRobotPose(const std::map& pose) { + RCLCPP_INFO(LOGGER, "updateRobotPose"); std::vector joint_names = jmg_->getActiveJointModelNames(); std::vector joints; if(utils::transcribeInputMap(pose, joint_names, joints)) diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index 30ed342a..49eb103a 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -50,7 +50,6 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt touch_links_.clear(); } -// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); if(!model_) @@ -77,11 +76,8 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt // Add the collision mesh object to the planning scene const std::string object_name = "reach_object"; -// std::string tmp_mesh_filename = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; -// const std::string tmp_mesh_filename = "/home/lovro/workspace/ros2_kortex_ws/src/reach/reach_demo/config/part.ply"; - const std::string tmp_mesh_filename = "package://reach_demo/config/part.ply"; - moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(tmp_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)) { RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); diff --git a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp index df092e9b..7a8d6183 100644 --- a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp @@ -40,10 +40,8 @@ bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr n return false; } -// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); - if(!model_) { RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); diff --git a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp index 041daad6..fbabfb66 100644 --- a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp @@ -39,9 +39,6 @@ bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr RCLCPP_ERROR(LOGGER, "MoveIt Manipulability Evaluation Plugin is missing 'planning_group' parameter"); return false; } - - RCLCPP_INFO(LOGGER, "Creating shared robot model in the node '%s' using parameter robot_description", node->get_name()); -// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); if(!model_) { diff --git a/reach_core/launch/start.launch.py b/reach_core/launch/start.launch.py index 42333d9b..bc6c640d 100644 --- a/reach_core/launch/start.launch.py +++ b/reach_core/launch/start.launch.py @@ -143,6 +143,58 @@ def generate_launch_description(): 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", @@ -152,7 +204,7 @@ def generate_launch_description(): parameters=[ robot_description, robot_description_semantic, - # ompl_planning_pipeline_config, + ompl_planning_pipeline_config, robot_description_kinematics, # robot_description_planning, ], @@ -162,6 +214,7 @@ def generate_launch_description(): 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_demo/config/params.yaml b/reach_demo/config/params.yaml index 4db1ec52..133cacf9 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -33,7 +33,8 @@ robot_reach_study_node: planning_group: "manipulator" distance_threshold: 0.025 exponent: 2 - collision_mesh_package: "reach_demo" +# 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: [""] @@ -41,7 +42,8 @@ robot_reach_study_node: display_config: name: "moveit_reach_plugins/display/MoveItReachDisplay" planning_group: "manipulator" - collision_mesh_package: "reach_demo" +# 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" 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 From be535d0d40416aefedaaa0a6c419611ff0a3e45e Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 11 Jan 2022 17:17:16 -0700 Subject: [PATCH 14/29] Update discretizied ik solver. --- .../src/ik/discretized_moveit_ik_solver.cpp | 1 - .../src/ik/moveit_ik_solver.cpp | 5 +- reach_core/launch/setup.launch.py | 26 -- reach_core/launch/start_demo.launch.py | 220 +++++++++++++++ reach_core/launch/start_rezilienth.launch.py | 257 ++++++++++++++++++ reach_demo/config/params.yaml | 3 +- 6 files changed, 480 insertions(+), 32 deletions(-) create mode 100644 reach_core/launch/start_demo.launch.py create mode 100644 reach_core/launch/start_rezilienth.launch.py 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 41f2881d..18ba996d 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -86,7 +86,6 @@ std::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen::Is { Eigen::Isometry3d discretized_target (target * Eigen::AngleAxisd (double(i)*dt_, Eigen::Vector3d::UnitZ())); std::vector tmp_solution; - std::optional score = MoveItIKSolver::solveIKFromSeed(discretized_target, seed, tmp_solution); if(score.has_value() && (score.value() > best_score)) { diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 599ec17e..ce62fc8d 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -104,10 +104,7 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) // Add the collision object to the planning scene const std::string object_name = "reach_object"; -// std::string mesh_path_tmp = ament_index_cpp::get_package_share_directory(collision_mesh_package_) + "/" + collision_mesh_filename_path_; -// const std::string mesh_path_tmp = "/home/lovro/workspace/ros2_kortex_ws/src/reach/reach_demo/config/part.ply"; - const std::string mesh_path_tmp = "package://reach_demo/config/part.ply"; - moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(mesh_path_tmp, collision_mesh_frame_, object_name); + moveit_msgs::msg::CollisionObject obj = utils::createCollisionObject(collision_mesh_package_, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) { RCLCPP_ERROR(LOGGER, "Failed to add collision mesh to planning scene"); diff --git a/reach_core/launch/setup.launch.py b/reach_core/launch/setup.launch.py index e6be2810..aa1e62de 100644 --- a/reach_core/launch/setup.launch.py +++ b/reach_core/launch/setup.launch.py @@ -21,33 +21,8 @@ def generate_launch_description(): default_value="true" ) ) - declared_arguments.append( - DeclareLaunchArgument( - "rviz_config_package", - description="Package where to find rviz file under /rviz subfolder.", - default_value="reach_core" - ) - ) visualize_results = LaunchConfiguration("visualize_results") - rviz_config_package = LaunchConfiguration("rviz_config_package") - - # rviz configuration - rviz_config_file = PathJoinSubstitution( - [FindPackageShare(rviz_config_package), - "rviz", - "reach_study_config.rviz"] - ) - - rviz_node = Node( - package="rviz2", - condition=IfCondition(visualize_results), - executable="rviz2", - name="rviz2_moveit", - output="log", - # arguments=["-d", rviz_config_file], - parameters=[], - ) load_point_cloud_server_node = Node( package="reach_core", @@ -58,7 +33,6 @@ def generate_launch_description(): ) nodes_to_run = [load_point_cloud_server_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..bc6c640d --- /dev/null +++ b/reach_core/launch/start_demo.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_rezilienth.launch.py b/reach_core/launch/start_rezilienth.launch.py new file mode 100644 index 00000000..2b953d92 --- /dev/null +++ b/reach_core/launch/start_rezilienth.launch.py @@ -0,0 +1,257 @@ +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="tele_exam_system_moveit_config" + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "parameters_filename", + description="YAML file for study parameters.", + default_value="reach_params.yaml" + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", + default_value="true", + description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument("xacro_file", + default_value="reach_study_tele_exam_system.xacro", + description="Xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument("description_package", + default_value="tele_exam_robot_description", + description="Xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument("moveit_config_file", + default_value="reach_study_tele_exam_system.srdf.xacro", + description="Moveit config xacro file to parse.") + ) + declared_arguments.append( + DeclareLaunchArgument("moveit_config_package", + default_value="tele_exam_system_moveit_config", + description="Moveit configuration package.") + ) + declared_arguments.append( + DeclareLaunchArgument( + "ros2_controllers_file", + default_value="ros2_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "include_patient_chair", + default_value="false", + description="To include patient and chair in the robot description?", + ) + ) + + parameters_package = LaunchConfiguration("parameters_package") + parameters_filename = LaunchConfiguration("parameters_filename") + moveit_config_file = LaunchConfiguration("moveit_config_file") + moveit_config_package = LaunchConfiguration("moveit_config_package") + ros2_controllers_file = LaunchConfiguration("ros2_controllers_file") + description_package = LaunchConfiguration("description_package") + include_patient_chair = LaunchConfiguration("include_patient_chair") + + study_parameters = PathJoinSubstitution( + [FindPackageShare(parameters_package), "config", parameters_filename] + ) + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(description_package), "urdf", LaunchConfiguration("xacro_file")] + ), + " ", + "include_patient_chair:=", + include_patient_chair, + " ", + ] + ) + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package),"config", moveit_config_file] + ), + ] + ) + + ros2_controllers = PathJoinSubstitution( + [FindPackageShare(parameters_package), "config", ros2_controllers_file] + ) + + kinematics_yaml = load_yaml("tele_exam_system_moveit_config", "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, ros2_controllers], + output={ + "stdout": "screen", + "stderr": "screen", + }, + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + jtc_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller", "--controller-manager", "/controller_manager"], + ) + gantry_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["gantry_joint_trajectory_controller", "--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( + "tele_exam_system_moveit_config", "config/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( + "tele_exam_system_moveit_config", "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, + # jtc_controller_spawner, + # gantry_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_demo/config/params.yaml b/reach_demo/config/params.yaml index 133cacf9..d2c1b5fd 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -20,7 +20,8 @@ robot_reach_study_node: name: "moveit_reach_plugins/ik/MoveItIKSolver" distance_threshold: 0.0 planning_group: "manipulator" - collision_mesh_package: "reach_demo" +# 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: [""] From f04751376db5f1240cda837c548e0b22ee72d377 Mon Sep 17 00:00:00 2001 From: Lovro Date: Thu, 13 Jan 2022 16:47:20 -0700 Subject: [PATCH 15/29] Show robot state in initial reach study. --- .../display/moveit_reach_display.h | 2 + .../src/display/moveit_reach_display.cpp | 12 ++-- .../src/ik/discretized_moveit_ik_solver.cpp | 4 +- .../src/ik/moveit_ik_solver.cpp | 2 + .../reach_core/plugins/reach_display_base.h | 2 + reach_core/include/reach_core/reach_study.h | 4 ++ reach_core/src/core/reach_study.cpp | 58 ++++++------------- .../src/load_point_cloud_server_node.cpp | 5 -- 8 files changed, 38 insertions(+), 51 deletions(-) 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 ec8dd91e..d6952e4d 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 @@ -56,6 +56,8 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase 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: diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index 7b156e02..9e91f540 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -97,19 +97,18 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n void MoveItReachDisplay::showEnvironment() { - while (scene_pub_->get_subscription_count() < 1) + while (scene_pub_->get_subscription_count() < 1) { RCLCPP_INFO(LOGGER, "No subscribers. Not showing environment..."); - rclcpp::sleep_for(std::chrono::milliseconds(500)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } - moveit_msgs::msg::PlanningScene scene_msg; + moveit_msgs::msg::PlanningScene scene_msg; scene_->getPlanningSceneMsg(scene_msg); scene_pub_->publish(scene_msg); } void MoveItReachDisplay::updateRobotPose(const std::map& pose) { - RCLCPP_INFO(LOGGER, "updateRobotPose"); std::vector joint_names = jmg_->getActiveJointModelNames(); std::vector joints; if(utils::transcribeInputMap(pose, joint_names, joints)) @@ -127,6 +126,11 @@ void MoveItReachDisplay::updateRobotPose(const std::map& po } } + void + MoveItReachDisplay::showEnvironment(const std::vector &names, const std::vector &positions) { + + } + } // namespace display } // namespace moveit_reach_plugins 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 18ba996d..2400605f 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -75,7 +75,9 @@ std::optional DiscretizedMoveItIKSolver::solveIKFromSeed(const Eigen::Is 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 diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index ce62fc8d..cda8824a 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -140,6 +140,8 @@ std::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& t // const static int SOLUTION_ATTEMPTS = 3; const static double SOLUTION_TIMEOUT = 0.2; +// RCLCPP_INFO(LOGGER, " TARGET: %f %f %f ", target.translation().x(), target.translation().y(),target.translation().z()); + if(state.setFromIK(jmg_, target, SOLUTION_TIMEOUT, std::bind(&MoveItIKSolver::isIKSolutionValid, this, std::placeholders::_1, 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 7782da19..2abf01f2 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -62,6 +62,8 @@ namespace reach 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) diff --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index 0e5d04c5..b616fffc 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -26,6 +26,8 @@ #include #include +#include "geometry_msgs/msg/pose_stamped.hpp" + namespace reach { namespace core @@ -95,6 +97,8 @@ namespace reach sensor_msgs::msg::PointCloud2 cloud_msg_; std::shared_ptr node_; + rclcpp::Publisher::SharedPtr ps_pub_; + }; } // namespace core diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 3af4b62a..67902dc5 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -65,7 +65,9 @@ namespace reach ik_solver_.reset(); display_.reset(); - try + ps_pub_ = node_->create_publisher("pose_stamped", 1); + + try { ik_solver_ = solver_loader_.createSharedInstance(sp_.ik_solver_config_name); display_ = display_loader_.createSharedInstance(sp_.display_config_name); @@ -87,8 +89,6 @@ namespace reach { RCLCPP_ERROR(LOGGER, "Could not initialized both display and ik solver plugins!"); return false; - }else { - RCLCPP_INFO(LOGGER, "IK and display solver successfully initialized!"); } display_->showEnvironment(); @@ -118,7 +118,7 @@ namespace reach bool ReachStudy::run(const StudyParameters &sp) { - // Overrwrite the old study parameters + // Overwrite the old study parameters sp_ = sp; // Initialize the study @@ -126,8 +126,6 @@ namespace reach { RCLCPP_ERROR(LOGGER, "Failed to initialize the reach study"); return false; - }else { - RCLCPP_INFO(LOGGER, "Reach study initialized!"); } // Get the reach object point cloud @@ -135,8 +133,6 @@ namespace reach { RCLCPP_ERROR(LOGGER, "Unable to obtain reach object point cloud"); return false; - }else { - RCLCPP_INFO(LOGGER, "Reach object point cloud obtained successfully!"); } // Show the reach object collision object and reach object point cloud @@ -144,13 +140,10 @@ namespace reach { rclcpp::Publisher::SharedPtr pub = node_->create_publisher(INPUT_CLOUD_TOPIC, 1); pub->publish(cloud_msg_); - }else { - RCLCPP_INFO(LOGGER, "Not visualizing results!"); } // Create markers visualizer_.reset(new ReachVisualizer(db_, ik_solver_, display_, sp_.optimization.radius)); -// RCLCPP_INFO(LOGGER, "Visualizer created!"); // Attempt to load previously saved optimized reach_study database if (!db_->load(results_dir_ + OPT_SAVED_DB_NAME)) @@ -178,7 +171,6 @@ namespace reach visualizer_->update(); } - RCLCPP_INFO(LOGGER, "Creating search tree!"); // Create an efficient search tree for doing nearest neighbors search search_tree_.reset(new SearchTree(flann::KDTreeSingleIndexParams(1, true))); @@ -215,7 +207,6 @@ namespace reach // Perform the calculation if it hasn't already been done if (db_->getStudyResults().avg_num_neighbors == 0.0f) { - RCLCPP_INFO(LOGGER, "Performing average neighbour calculation.") ; getAverageNeighborsCount(); } } @@ -240,9 +231,9 @@ namespace reach { // Call the sample mesh service to create a point cloud of the reach object mesh 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_); // auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC); - auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC, rmw_qos_profile_services_default, callback_group_input_); -// get_input_client_ = node_->create_client("GetInput", rmw_qos_profile_services_default, callback_group_input_); +// get_input_client_ = node_->create_client("GetInput", rmw_qos_profile_services_default, callback_group_input_); auto req = std::make_shared(); req->cloud_filename = ament_index_cpp::get_package_share_directory(sp_.pcd_package) + "/" + sp_.pcd_filename_path; @@ -254,7 +245,6 @@ namespace reach // auto result = client->async_send_request(req); bool success_tmp = false; - auto inner_client_callback = [&,this](rclcpp::Client::SharedFuture inner_future) { RCLCPP_INFO(LOGGER, "Inner service callback started"); @@ -282,32 +272,8 @@ namespace reach << "'"); return false; } - - // Wait for the result. - { -// if (rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result) == -// rclcpp::FutureReturnCode::SUCCESS) { -// if (!result.get()->success) { -// RCLCPP_ERROR_STREAM(LOGGER, result.get()->message); -// return false; -// } -// -// cloud_msg_ = result.get()->cloud; -// pcl::fromROSMsg(cloud_msg_, *cloud_); -// -// cloud_msg_.header.frame_id = sp_.fixed_frame; -// cloud_msg_.header.stamp = node_->now(); -// -// return true; -// } else { -// RCLCPP_ERROR_STREAM(LOGGER, "Failed to call point cloud loading service '" << client->get_service_name() -// << "'"); -// return false; -// } } - } - void ReachStudy::runInitialReachStudy() { // Rotation to flip the Z axis of the surface normal point @@ -340,10 +306,20 @@ namespace reach geometry_msgs::msg::Pose tgt_pose; tgt_pose = tf2::toMsg(tgt_frame); - sensor_msgs::msg::JointState goal_state(seed_state); + 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); + sensor_msgs::msg::JointState goal_state(seed_state); if (score) { + std::map robot_configuration; + for (size_t i = 0; i< seed_state.name.size(); ++i){ + robot_configuration[seed_state.name[i]] = solution[i]; + } + 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); diff --git a/reach_core/src/load_point_cloud_server_node.cpp b/reach_core/src/load_point_cloud_server_node.cpp index 638ca2bf..9f176d3f 100644 --- a/reach_core/src/load_point_cloud_server_node.cpp +++ b/reach_core/src/load_point_cloud_server_node.cpp @@ -109,11 +109,6 @@ using LoadPCLResSharedPtr = LoadPCLRes::SharedPtr; // Convert point cloud to message for output sensor_msgs::msg::PointCloud2 msg; pcl::toROSMsg(transformed_cloud, res->cloud); -// for(size_t i = 0; i < res->cloud.data.size(); ++i){ -// if (res->cloud.data[i]!= 0.0) { -// RCLCPP_INFO(this->get_logger(), "%d-th data = %f ", i, res->cloud.data[i]); -// } -// } res->success = true; res->message = "Successfully loaded point cloud from '" + req->cloud_filename + "'"; From 39bc330c2500ae83667723fa941bc73deb484b87 Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 25 Jan 2022 12:17:53 +0100 Subject: [PATCH 16/29] Make robot state publish in ik valid cb. --- .../display/moveit_reach_display.h | 3 - .../ik/moveit_ik_solver.h | 5 + .../src/display/moveit_reach_display.cpp | 17 +- .../src/ik/moveit_ik_solver.cpp | 17 +- .../reach_core/plugins/ik_solver_base.h | 4 + .../reach_core/plugins/reach_display_base.h | 4 +- reach_core/include/reach_core/reach_study.h | 1 + reach_core/launch/start_demo.launch.py | 5 +- reach_core/launch/start_rezilienth.launch.py | 257 ------------- reach_core/rviz/reach_study_config.rviz | 349 +++++++++++++++--- reach_core/src/core/ik_helper.cpp | 3 - reach_core/src/core/reach_database.cpp | 4 +- reach_core/src/core/reach_study.cpp | 29 +- 13 files changed, 341 insertions(+), 357 deletions(-) delete mode 100644 reach_core/launch/start_rezilienth.launch.py 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 d6952e4d..8a16349e 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 @@ -70,11 +70,8 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase std::string collision_mesh_package_; std::string collision_mesh_filename_path_; - std::string collision_mesh_frame_; - rclcpp::Node::SharedPtr n_; - rclcpp::Publisher::SharedPtr scene_pub_; }; 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 55345047..1bbd4dbe 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 @@ -20,6 +20,9 @@ #include #include +// PlanningScene +#include + namespace moveit { namespace core @@ -80,6 +83,8 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase 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/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index 9e91f540..f546a7ce 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -19,6 +19,10 @@ #include #include +// conversions +#include +#include + const static std::string PLANNING_SCENE_TOPIC = "planning_scene_display"; namespace moveit_reach_plugins @@ -35,9 +39,10 @@ MoveItReachDisplay::MoveItReachDisplay() bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node) { RCLCPP_INFO(LOGGER, "Initializing MoveItReachDisplay!"); - reach::plugins::DisplayBase::initialize(name, node); - - n_ = node; + if (!reach::plugins::DisplayBase::initialize(name, node)) + { + return false; + } std::string param_prefix("display_config."); std::string planning_group; @@ -97,11 +102,6 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n void MoveItReachDisplay::showEnvironment() { - while (scene_pub_->get_subscription_count() < 1) - { - RCLCPP_INFO(LOGGER, "No subscribers. Not showing environment..."); - rclcpp::sleep_for(std::chrono::milliseconds(100)); - } moveit_msgs::msg::PlanningScene scene_msg; scene_->getPlanningSceneMsg(scene_msg); scene_pub_->publish(scene_msg); @@ -110,6 +110,7 @@ void MoveItReachDisplay::showEnvironment() void MoveItReachDisplay::updateRobotPose(const std::map& pose) { std::vector joint_names = jmg_->getActiveJointModelNames(); + std::vector joints; if(utils::transcribeInputMap(pose, joint_names, joints)) { diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index cda8824a..989b82f1 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -36,6 +36,8 @@ MoveItIKSolver::MoveItIKSolver() bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) { + node_ = node; + std::string planning_group; if(!node->get_parameter("ik_solver_config.planning_group", planning_group) || @@ -76,8 +78,6 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) return false; } - RCLCPP_INFO(LOGGER, "Initializing robot shared model"); -// model_ = moveit::planning_interface::getSharedRobotModel(node, "robot_description"); model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); if(!model_) @@ -102,7 +102,10 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) 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::msg::CollisionObject obj = utils::createCollisionObject(collision_mesh_package_, collision_mesh_frame_, object_name); if(!scene_->processCollisionObjectMsg(obj)) @@ -140,8 +143,6 @@ std::optional MoveItIKSolver::solveIKFromSeed(const Eigen::Isometry3d& t // const static int SOLUTION_ATTEMPTS = 3; const static double SOLUTION_TIMEOUT = 0.2; -// RCLCPP_INFO(LOGGER, " TARGET: %f %f %f ", target.translation().x(), target.translation().y(),target.translation().z()); - if(state.setFromIK(jmg_, target, SOLUTION_TIMEOUT, std::bind(&MoveItIKSolver::isIKSolutionValid, this, std::placeholders::_1, @@ -176,6 +177,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); } 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 d6220f4a..6df4036a 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -65,6 +65,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/reach_display_base.h b/reach_core/include/reach_core/plugins/reach_display_base.h index 2abf01f2..5435b6ad 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -22,10 +22,13 @@ #include #include "reach_core/utils/visualization_utils.h" #include +// PoseStamped +#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 { @@ -214,7 +217,6 @@ namespace reach std::shared_ptr> diff_pub_; std::shared_ptr> marker_pub_; - }; typedef std::shared_ptr DisplayBasePtr; diff --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index b616fffc..08d42f04 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -27,6 +27,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" namespace reach { diff --git a/reach_core/launch/start_demo.launch.py b/reach_core/launch/start_demo.launch.py index bc6c640d..97114aee 100644 --- a/reach_core/launch/start_demo.launch.py +++ b/reach_core/launch/start_demo.launch.py @@ -195,12 +195,15 @@ def generate_launch_description(): ], ) + 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], + arguments=["-d", rviz_config_file], parameters=[ robot_description, robot_description_semantic, diff --git a/reach_core/launch/start_rezilienth.launch.py b/reach_core/launch/start_rezilienth.launch.py deleted file mode 100644 index 2b953d92..00000000 --- a/reach_core/launch/start_rezilienth.launch.py +++ /dev/null @@ -1,257 +0,0 @@ -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="tele_exam_system_moveit_config" - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "parameters_filename", - description="YAML file for study parameters.", - default_value="reach_params.yaml" - ) - ) - declared_arguments.append( - DeclareLaunchArgument("launch_rviz", - default_value="true", - description="Launch RViz?") - ) - declared_arguments.append( - DeclareLaunchArgument("xacro_file", - default_value="reach_study_tele_exam_system.xacro", - description="Xacro file to parse.") - ) - declared_arguments.append( - DeclareLaunchArgument("description_package", - default_value="tele_exam_robot_description", - description="Xacro file to parse.") - ) - declared_arguments.append( - DeclareLaunchArgument("moveit_config_file", - default_value="reach_study_tele_exam_system.srdf.xacro", - description="Moveit config xacro file to parse.") - ) - declared_arguments.append( - DeclareLaunchArgument("moveit_config_package", - default_value="tele_exam_system_moveit_config", - description="Moveit configuration package.") - ) - declared_arguments.append( - DeclareLaunchArgument( - "ros2_controllers_file", - default_value="ros2_controllers.yaml", - description="YAML file with the controllers configuration.", - ) - ) - declared_arguments.append( - DeclareLaunchArgument( - "include_patient_chair", - default_value="false", - description="To include patient and chair in the robot description?", - ) - ) - - parameters_package = LaunchConfiguration("parameters_package") - parameters_filename = LaunchConfiguration("parameters_filename") - moveit_config_file = LaunchConfiguration("moveit_config_file") - moveit_config_package = LaunchConfiguration("moveit_config_package") - ros2_controllers_file = LaunchConfiguration("ros2_controllers_file") - description_package = LaunchConfiguration("description_package") - include_patient_chair = LaunchConfiguration("include_patient_chair") - - study_parameters = PathJoinSubstitution( - [FindPackageShare(parameters_package), "config", parameters_filename] - ) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(description_package), "urdf", LaunchConfiguration("xacro_file")] - ), - " ", - "include_patient_chair:=", - include_patient_chair, - " ", - ] - ) - # MoveIt Configuration - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_config_package),"config", moveit_config_file] - ), - ] - ) - - ros2_controllers = PathJoinSubstitution( - [FindPackageShare(parameters_package), "config", ros2_controllers_file] - ) - - kinematics_yaml = load_yaml("tele_exam_system_moveit_config", "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, ros2_controllers], - output={ - "stdout": "screen", - "stderr": "screen", - }, - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], - ) - jtc_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_trajectory_controller", "--controller-manager", "/controller_manager"], - ) - gantry_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["gantry_joint_trajectory_controller", "--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( - "tele_exam_system_moveit_config", "config/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( - "tele_exam_system_moveit_config", "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, - # jtc_controller_spawner, - # gantry_controller_spawner, - ] - - return LaunchDescription(declared_arguments + nodes_to_run) \ No newline at end of file diff --git a/reach_core/rviz/reach_study_config.rviz b/reach_core/rviz/reach_study_config.rviz index 70b65099..e91d13f0 100644 --- a/reach_core/rviz/reach_study_config.rviz +++ b/reach_core/rviz/reach_study_config.rviz @@ -1,41 +1,44 @@ Panels: - - Class: rviz/Displays - Help Height: 78 + - Class: rviz_common/Displays + Help Height: 87 Name: Displays Property Tree Widget: Expanded: - - /MarkerArray1/Namespaces1 - Splitter Ratio: 0.5271919965744019 - Tree Height: 695 - - Class: rviz/Selection + - /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/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 + SyncSource: PointCloud2 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -51,39 +54,94 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: 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: false + Show Axes: true Show Descriptions: true Show Visual Aids: false - Update Topic: /reach_int_markers/update Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /reach_neighbors + - Class: rviz_default_plugins/Marker + Enabled: false Name: Marker Namespaces: {} - Queue Size: 100 - Value: true - - Class: rviz/MarkerArray + 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 - Marker Topic: /reach_comparison Name: MarkerArray Namespaces: {} - Queue Size: 100 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /reach_neighbors_array Value: false - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: false Frame Timeout: 15 Frames: - All Enabled: true + All Enabled: false Marker Scale: 1 Name: TF - Show Arrows: true + Show Arrows: false Show Axes: true Show Names: true Tree: @@ -97,9 +155,9 @@ Visualization Manager: Planning Scene Topic: /planning_scene_display Robot Description: robot_description Scene Geometry: - Scene Alpha: 1 + Scene Alpha: 0.8999999761581421 Scene Color: 50; 230; 50 - Scene Display Time: 0.20000000298023224 + Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels @@ -171,61 +229,230 @@ Visualization Manager: 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 - Default Light: true Fixed Frame: base_link Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/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 + - 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: /clicked_point + 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/Orbit - Distance: 11.848441123962402 + 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.241093710064888 - Y: 0.3581845760345459 - Z: 1.6464532613754272 + 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.3263669013977051 + Pitch: 0.5002043843269348 Target Frame: Value: Orbit (rviz) - Yaw: 2.227778196334839 + Yaw: 1.6823487281799316 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 992 + Height: 1129 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000019e00000342fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000004bb0000028200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000342000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000420000000160000000000000000000000010000010f000002f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002f6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005990000034200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c7000003a7fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000003a7000000fd01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005001000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000215000001d6000001ac01000003fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000039b0000005000000050010000030000000100000110000003a8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000044000003a8000000d301000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000056fc0100000002fb0000000800540069006d0065010000000000000780000002ad01000003fb0000000800540069006d00650100000000000004500000000000000000000005b8000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false Selection: collapsed: false Time: @@ -234,6 +461,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 - Y: 27 + 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 a59b5ac0..5cd3c754 100644 --- a/reach_core/src/core/ik_helper.cpp +++ b/reach_core/src/core/ik_helper.cpp @@ -184,12 +184,9 @@ namespace reach tf2::fromMsg(neighbors[i].goal, target); // Use current point's IK solution as seed -// RCLCPP_INFO(rclcpp::get_logger("ik_helper"), "Before solve..."); std::optional score = solver->solveIKFromSeed(target, current_pose_map, new_pose); -// RCLCPP_INFO(rclcpp::get_logger("ik_helper"), "After solve..."); if (score) { -// RCLCPP_INFO(rclcpp::get_logger("ik_helper"), "Score exists..."); // Calculate the joint distance between the seed and new goal states for (std::size_t j = 0; j < current_pose.size(); ++j) { diff --git a/reach_core/src/core/reach_database.cpp b/reach_core/src/core/reach_database.cpp index 641eb33b..5a714b47 100644 --- a/reach_core/src/core/reach_database.cpp +++ b/reach_core/src/core/reach_database.cpp @@ -170,8 +170,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 67902dc5..1c596f33 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -232,8 +232,6 @@ namespace reach // Call the sample mesh service to create a point cloud of the reach object mesh 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_); -// auto client = node_->create_client(SAMPLE_MESH_SRV_TOPIC); -// get_input_client_ = node_->create_client("GetInput", rmw_qos_profile_services_default, callback_group_input_); auto req = std::make_shared(); req->cloud_filename = ament_index_cpp::get_package_share_directory(sp_.pcd_package) + "/" + sp_.pcd_filename_path; @@ -242,7 +240,6 @@ namespace reach RCLCPP_INFO(LOGGER, "Waiting for service '%s'.", SAMPLE_MESH_SRV_TOPIC); client->wait_for_service(); -// auto result = client->async_send_request(req); bool success_tmp = false; auto inner_client_callback = [&,this](rclcpp::Client::SharedFuture inner_future) @@ -306,19 +303,23 @@ namespace reach geometry_msgs::msg::Pose tgt_pose; tgt_pose = tf2::toMsg(tgt_frame); - 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); - sensor_msgs::msg::JointState goal_state(seed_state); + sensor_msgs::msg::JointState goal_state(seed_state); 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; - for (size_t i = 0; i< seed_state.name.size(); ++i){ - robot_configuration[seed_state.name[i]] = solution[i]; - } + // 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); @@ -406,19 +407,15 @@ namespace reach current_counter = previous_pct = neighbor_count = 0; std::atomic total_joint_distance; const int total = db_->size(); - int calc = 0; // Iterate #pragma parallel for for (auto it = db_->begin(); it != db_->end(); ++it) { -// RCLCPP_INFO(LOGGER, "Calculation no %d", calc++); reach_msgs::msg::ReachRecord msg = it->second; if (msg.reached) { NeighborReachResult result; -// RCLCPP_INFO(LOGGER, "Before recursion..."); reachNeighborsRecursive(db_, msg, ik_solver_, sp_.optimization.radius, result, search_tree_); -// RCLCPP_INFO(LOGGER, "After recursion..."); neighbor_count += static_cast(result.reached_pts.size() - 1); total_joint_distance = total_joint_distance + result.joint_distance; } From 77a0c88bb5d87944eded9d9be4b39273d3f3ccb1 Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 31 Jan 2022 12:44:45 +0100 Subject: [PATCH 17/29] Add flag for inner service callback. --- reach_core/src/core/reach_study.cpp | 10 ++++++---- reach_core/src/robot_reach_study_node.cpp | 12 ------------ 2 files changed, 6 insertions(+), 16 deletions(-) diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 1c596f33..e21e0b05 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -240,21 +240,23 @@ namespace reach RCLCPP_INFO(LOGGER, "Waiting for service '%s'.", SAMPLE_MESH_SRV_TOPIC); client->wait_for_service(); - bool success_tmp = false; + bool success_tmp = false; + bool inner_callback_finished = false; auto inner_client_callback = [&,this](rclcpp::Client::SharedFuture inner_future) { - RCLCPP_INFO(LOGGER, "Inner service callback started"); 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()); - RCLCPP_INFO(LOGGER, "Inner service callback finished"); + 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 - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + while(!inner_callback_finished) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } if (success_tmp){ pcl::fromROSMsg(cloud_msg_, *cloud_); diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 3de6601d..d75d95f7 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -97,7 +97,6 @@ int main(int argc, char **argv) rclcpp::executors::MultiThreadedExecutor executor; - // create node auto node = std::make_shared("robot_reach_study_node"); @@ -112,18 +111,7 @@ int main(int argc, char **argv) std::thread t1( [&executor]{ // spin -// rclcpp::spin(node); executor.spin(); - -// rclcpp::WallRate loop_rate(100); -// while (rclcpp::ok()) { -// -//// executor.spin_once(); -// rclcpp::spin_some(node); -// loop_rate.sleep(); -// } - - }); // Initialize the reach study From 881a83421c88b48acd2b6cb89c7caa4c575973c6 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 2 Feb 2022 14:05:40 +0100 Subject: [PATCH 18/29] Update data loader node. --- reach_core/src/core/reach_database.cpp | 2 - reach_core/src/data_loader_node.cpp | 72 ++++++++++++++++++++++---- 2 files changed, 63 insertions(+), 11 deletions(-) diff --git a/reach_core/src/core/reach_database.cpp b/reach_core/src/core/reach_database.cpp index 5a714b47..c2091f60 100644 --- a/reach_core/src/core/reach_database.cpp +++ b/reach_core/src/core/reach_database.cpp @@ -89,14 +89,12 @@ namespace reach bool ReachDatabase::load(const std::string &filename) { - RCLCPP_INFO(LOGGER, "ReachDatabase::load from '%s'", filename.c_str()); 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; } - RCLCPP_INFO(LOGGER, "ReachDatabase::load ==> loaded from file successfully!"); std::lock_guard lock{mutex_}; for (const auto &r : msg.records) diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index 5d9ecda2..4a163c7b 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -49,27 +49,79 @@ bool get_all(const std::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; } +//bool get_all_subdirs(const std::filesystem::path& root, +// const std::string& ext, +// std::vector>& ret) +//{ +// if(!std::filesystem::exists(root)) return false; +// +// if(!std::filesystem::is_directory(root)) return false; +// +// std::filesystem::recursive_directory_iterator it(root); +// std::filesystem::recursive_directory_iterator endit; +// +// while(it != endit) +// { +// printf("%s\n", it->path().filename().c_str()); +// 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; +// tmp.first = it->path().parent_path().filename(); +// tmp.second = it->path(); +// ret.push_back(tmp); +// } +// } +// ++it; +// } +// +// std::sort(ret.begin(), ret.end()); +// +// 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"); + auto node = std::make_shared("data_loader_node", options); + std::string pkg_name; + std::string dir_name; + bool chk_all_sub_dirs; - std::string root_path = std::string(ament_index_cpp::get_package_share_directory(("reach_core"))) + "/" + RESULTS_FOLDER_NAME; + 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); - if(argv[1]) + + std::string root_path = std::string(ament_index_cpp::get_package_share_directory(pkg_name)) + "/" + dir_name; + + if(argv[1] && !chk_all_sub_dirs) { const std::string folder_name = argv[1]; root_path += "/" + folder_name; @@ -77,12 +129,14 @@ int main(int argc, char **argv) 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" From bc3f89862b7e6ba501834f2552de5f6b3f6dceda Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 2 Feb 2022 14:11:35 +0100 Subject: [PATCH 19/29] Optional formatting. --- reach_core/src/data_loader_node.cpp | 41 ++++++++++++++++++++--------- 1 file changed, 28 insertions(+), 13 deletions(-) diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index 4a163c7b..18e84057 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -113,10 +113,12 @@ int main(int argc, char **argv) std::string pkg_name; std::string dir_name; bool chk_all_sub_dirs; + bool avg_neighbor_count; 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); std::string root_path = std::string(ament_index_cpp::get_package_share_directory(pkg_name)) + "/" + dir_name; @@ -136,13 +138,19 @@ int main(int argc, char **argv) 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"; + if (avg_neighbor_count) { + std::cout << boost::format("%-30s %=25s %=25s %=25s %=25s\n") + % "Configuration Name" + % "Reach Percentage" + % "Normalized Total Pose Score" + % "Average Reachable Neighbors" + % "Average Joint Distance"; + }else { + std::cout << boost::format("%-30s %=25s %=25s\n") + % "Configuration Name" + % "Reach Percentage" + % "Normalized Total Pose Score"; + } for(size_t i = 0; i < files.size(); ++i) { @@ -153,12 +161,19 @@ int main(int argc, char **argv) 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; + if(avg_neighbor_count) { + 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; + }else { + std::cout << boost::format("%-30s %=25.3f %=25.6f\n") + % config.c_str() + % res.reach_percentage + % res.norm_total_pose_score; + } } } // shutdown From 49e60563da83f6ae4b226bd5b510cef6871e3a08 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 2 Feb 2022 16:51:58 +0100 Subject: [PATCH 20/29] Change constructing objects. --- moveit_reach_plugins/src/display/moveit_reach_display.cpp | 2 +- moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp | 2 +- moveit_reach_plugins/src/ik/moveit_ik_solver.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index f546a7ce..2df2bdca 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -73,7 +73,7 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n 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_)) diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index 49eb103a..a5e7c094 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -65,7 +65,7 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt 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_)) diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 989b82f1..55ed6151 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -93,7 +93,7 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) 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_)) From c7457a658800bf4531ae19d6272dd3afca6259a2 Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 2 Feb 2022 16:53:25 +0100 Subject: [PATCH 21/29] Enable openmp. --- reach_core/CMakeLists.txt | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index 31597cc8..6e298b40 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -18,14 +18,13 @@ find_package(reach_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(visualization_msgs REQUIRED) -#find_package(fmt REQUIRED) -#find_package(OpenMP) -#if(OPENMP_FOUND) -# set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -# set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") -#endif() +find_package(OpenMP) +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +endif() set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs @@ -33,7 +32,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_core # pcl_ros pcl_conversions - pluginlib +# pluginlib rclcpp reach_msgs tf2_eigen @@ -128,9 +127,10 @@ target_link_libraries(robot_reach_study_node ${PROJECT_NAME}_utils ${PROJECT_NAME}_plugins ) -ament_target_dependencies(robot_reach_study_node - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) +#ament_target_dependencies(robot_reach_study_node +# ${THIS_PACKAGE_INCLUDE_DEPENDS} +# pluginlib +#) ## Load Point Cloud Server Node add_executable(load_point_cloud_server_node From dc6d80bbf68b101b7008a1b772f12637a087867c Mon Sep 17 00:00:00 2001 From: Lovro Date: Wed, 2 Feb 2022 17:02:54 +0100 Subject: [PATCH 22/29] Remove objects. --- moveit_reach_plugins/src/ik/moveit_ik_solver.cpp | 12 ++++++------ .../reach_core/plugins/impl/multiplicative_factory.h | 6 ++++++ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 55ed6151..c867ac87 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -177,12 +177,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); - } +// 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); } 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 f1e00b19..55790cc7 100644 --- a/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h +++ b/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h @@ -29,6 +29,12 @@ namespace reach public: MultiplicativeFactory(); + ~MultiplicativeFactory(){ + for(auto &ev_pl : eval_plugins_){ + ev_pl.reset(); + } + } + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; virtual double calculateScore(const std::map &pose) override; From f94ae77e1869dd0559de78f64a41a6721e9fdd14 Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 14 Feb 2022 15:56:44 +0100 Subject: [PATCH 23/29] Add robot model ptr to initialize method. --- .../display/moveit_reach_display.h | 14 ++------ .../evaluation/distance_penalty_moveit.h | 14 ++------ .../evaluation/joint_penalty_moveit.h | 13 ++------ .../evaluation/manipulability_moveit.h | 14 ++------ .../ik/discretized_moveit_ik_solver.h | 2 +- .../ik/moveit_ik_solver.h | 28 +++++++++------- .../src/display/moveit_reach_display.cpp | 7 ++-- .../evaluation/distance_penalty_moveit.cpp | 5 +-- .../src/evaluation/joint_penalty_moveit.cpp | 5 +-- .../src/evaluation/manipulability_moveit.cpp | 5 +-- .../src/ik/discretized_moveit_ik_solver.cpp | 4 +-- .../src/ik/moveit_ik_solver.cpp | 7 ++-- reach_core/CMakeLists.txt | 13 ++++---- .../reach_core/plugins/evaluation_base.h | 5 ++- .../reach_core/plugins/ik_solver_base.h | 3 +- .../plugins/impl/multiplicative_factory.h | 2 +- .../reach_core/plugins/reach_display_base.h | 4 ++- reach_core/include/reach_core/reach_study.h | 21 ++++++++++-- .../include/reach_core/study_parameters.h | 1 + reach_core/src/core/reach_study.cpp | 33 +++++++++++++++---- .../plugins/impl/multiplicative_factory.cpp | 4 +-- reach_core/src/robot_reach_study_node.cpp | 1 + reach_demo/config/params.yaml | 1 + 23 files changed, 110 insertions(+), 96 deletions(-) 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 8a16349e..8f1d004b 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 @@ -21,16 +21,6 @@ #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} - namespace planning_scene { class PlanningScene; @@ -52,7 +42,7 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase MoveItReachDisplay(); - bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; + bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual void showEnvironment() override; @@ -62,7 +52,7 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase private: - moveit::core::RobotModelConstPtr model_; + moveit::core::RobotModelPtr model_; planning_scene::PlanningScenePtr scene_; 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 6553063c..f20d7227 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 @@ -19,16 +19,6 @@ #include #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} - namespace planning_scene { class PlanningScene; @@ -50,13 +40,13 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase DistancePenaltyMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) 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; private: - moveit::core::RobotModelConstPtr model_; + moveit::core::RobotModelPtr model_; const moveit::core::JointModelGroup* jmg_; 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 0296aa21..f4959f51 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,15 +18,6 @@ #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} namespace moveit_reach_plugins { @@ -43,7 +34,7 @@ class JointPenaltyMoveIt : public reach::plugins::EvaluationBase JointPenaltyMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) 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; @@ -51,7 +42,7 @@ class JointPenaltyMoveIt : public reach::plugins::EvaluationBase std::vector> getJointLimits(); - moveit::core::RobotModelConstPtr model_; + moveit::core::RobotModelPtr model_; const moveit::core::JointModelGroup* jmg_; 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 688ee5f8..cd230e0a 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,16 +18,6 @@ #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -} -} - namespace moveit_reach_plugins { namespace @@ -43,13 +33,13 @@ class ManipulabilityMoveIt : public reach::plugins::EvaluationBase ManipulabilityMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) 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; private: - moveit::core::RobotModelConstPtr model_; + moveit::core::RobotModelPtr model_; const moveit::core::JointModelGroup* jmg_; }; 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 b8da0122..14b75661 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 @@ -30,7 +30,7 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver DiscretizedMoveItIKSolver(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map& seed, 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 1bbd4dbe..3e7c9c7e 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 @@ -23,16 +23,16 @@ // PlanningScene #include -namespace moveit -{ -namespace core -{ -class RobotModel; -typedef std::shared_ptr RobotModelConstPtr; -class JointModelGroup; -class RobotState; -} -} +//namespace moveit +//{ +//namespace core +//{ +//class RobotModel; +//typedef std::shared_ptr RobotModelConstPtr; +//class JointModelGroup; +//class RobotState; +//} +//} namespace planning_scene { @@ -55,7 +55,11 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase MoveItIKSolver(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; + ~MoveItIKSolver(){ + eval_.reset(); + } + + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map &seed, @@ -69,7 +73,7 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase const moveit::core::JointModelGroup* jmg, const double* ik_solution) const; - moveit::core::RobotModelConstPtr model_; + moveit::core::RobotModelPtr model_; planning_scene::PlanningScenePtr scene_; const moveit::core::JointModelGroup* jmg_; diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index 2df2bdca..fd5a4edc 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -36,10 +36,10 @@ MoveItReachDisplay::MoveItReachDisplay() } -bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node) +bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { RCLCPP_INFO(LOGGER, "Initializing MoveItReachDisplay!"); - if (!reach::plugins::DisplayBase::initialize(name, node)) + if (!reach::plugins::DisplayBase::initialize(name, node, model)) { return false; } @@ -58,7 +58,8 @@ bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr n return false; } - model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; if(!model_) { diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index a5e7c094..b8f76e63 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -30,7 +30,7 @@ DistancePenaltyMoveIt::DistancePenaltyMoveIt() } -bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node) +bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { std::string planning_group; @@ -50,7 +50,8 @@ bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPt touch_links_.clear(); } - model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; if(!model_) { diff --git a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp index 7a8d6183..5ccac052 100644 --- a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp @@ -29,7 +29,7 @@ JointPenaltyMoveIt::JointPenaltyMoveIt() } -bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node) +bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { std::string planning_group; @@ -40,7 +40,8 @@ bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr n return false; } - model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; if(!model_) { diff --git a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp index fbabfb66..ec43421d 100644 --- a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp @@ -29,7 +29,7 @@ ManipulabilityMoveIt::ManipulabilityMoveIt() } -bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node) +bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { std::string planning_group; @@ -39,7 +39,8 @@ bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr RCLCPP_ERROR(LOGGER, "MoveIt Manipulability Evaluation Plugin is missing 'planning_group' parameter"); return false; } - model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; if(!model_) { RCLCPP_ERROR(LOGGER, "Failed to initialize robot model pointer"); 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 2400605f..1a4d1602 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -40,9 +40,9 @@ DiscretizedMoveItIKSolver::DiscretizedMoveItIKSolver() } -bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) +bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { - if(!MoveItIKSolver::initialize(name, node)) + if(!MoveItIKSolver::initialize(name, node, model)) { RCLCPP_ERROR(LOGGER, "Failed to initialize MoveItIKSolver plugin"); return false; diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index c867ac87..86ef4369 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -34,7 +34,7 @@ MoveItIKSolver::MoveItIKSolver() } -bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) +bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { node_ = node; @@ -66,7 +66,7 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) } try { - if(!eval_->initialize(evaluation_plugin_name_, node)) + if(!eval_->initialize(evaluation_plugin_name_, node, model)) { RCLCPP_ERROR_STREAM(LOGGER, "Failed to initialize evaluation plugin"); return false; @@ -78,7 +78,8 @@ bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node) return false; } - model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); +// model_ = moveit::planning_interface::getSharedRobotModelLoader(node, "robot_description")->getModel(); + model_ = model; if(!model_) { diff --git a/reach_core/CMakeLists.txt b/reach_core/CMakeLists.txt index 6e298b40..80fc95b1 100644 --- a/reach_core/CMakeLists.txt +++ b/reach_core/CMakeLists.txt @@ -18,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) @@ -38,6 +40,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS tf2_eigen tf2_ros visualization_msgs + moveit_ros_planning_interface ) ########### @@ -47,6 +50,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS include_directories( include ${PCL_INCLUDE_DIRS} + ${moveit_ros_planning_interface_INCLUDE_DIRS} ) # Plugins Library @@ -108,7 +112,7 @@ target_include_directories(${PROJECT_NAME} $ $ ) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_plugins ${PROJECT_NAME}_utils) +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} ) @@ -125,12 +129,8 @@ target_include_directories(robot_reach_study_node target_link_libraries(robot_reach_study_node ${PROJECT_NAME} ${PROJECT_NAME}_utils - ${PROJECT_NAME}_plugins +# ${PROJECT_NAME}_plugins li promijenio ) -#ament_target_dependencies(robot_reach_study_node -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# pluginlib -#) ## Load Point Cloud Server Node add_executable(load_point_cloud_server_node @@ -140,7 +140,6 @@ 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} ) diff --git a/reach_core/include/reach_core/plugins/evaluation_base.h b/reach_core/include/reach_core/plugins/evaluation_base.h index 7a283a4d..a1752525 100644 --- a/reach_core/include/reach_core/plugins/evaluation_base.h +++ b/reach_core/include/reach_core/plugins/evaluation_base.h @@ -21,6 +21,9 @@ #include +#include + + namespace reach { namespace plugins @@ -44,7 +47,7 @@ namespace reach * @brief initialize * @param config */ - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) = 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 6df4036a..017eeb00 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -22,6 +22,7 @@ #include #include +#include namespace reach { @@ -47,7 +48,7 @@ namespace reach * @param config * @return */ - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) = 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. 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 55790cc7..25f773de 100644 --- a/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h +++ b/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h @@ -35,7 +35,7 @@ namespace reach } } - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node) override; + 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 5435b6ad..e655ff0d 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -25,6 +25,8 @@ // PoseStamped #include +#include + constexpr char INTERACTIVE_MARKER_TOPIC[] = "reach_int_markers"; constexpr char REACH_DIFF_TOPIC[] = "reach_comparison"; constexpr char MARKER_TOPIC[] = "reach_neighbors"; @@ -53,7 +55,7 @@ namespace reach marker_pub_.reset(); } - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node){ + 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 --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index 08d42f04..24f4aa5f 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -21,7 +21,6 @@ #include #include #include -// #include #include #include #include @@ -29,6 +28,19 @@ #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 { namespace core @@ -46,6 +58,8 @@ namespace reach */ ReachStudy(const rclcpp::Node::SharedPtr node); + ~ReachStudy(); + /** * @brief run * @param sp @@ -63,7 +77,7 @@ namespace reach } private: - bool initializeStudy(); + bool initializeStudy(const StudyParameters &sp); bool getReachObjectPointCloud(); @@ -100,6 +114,9 @@ namespace reach std::shared_ptr node_; rclcpp::Publisher::SharedPtr ps_pub_; + // robot model + moveit::core::RobotModelPtr model_; + }; } // namespace core diff --git a/reach_core/include/reach_core/study_parameters.h b/reach_core/include/reach_core/study_parameters.h index eaf8574a..d369fdac 100644 --- a/reach_core/include/reach_core/study_parameters.h +++ b/reach_core/include/reach_core/study_parameters.h @@ -64,6 +64,7 @@ struct StudyParameters std::vector compare_dbs; std::string fixed_frame; std::string object_frame; + std::string planning_group; }; } // namespace core diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index e21e0b05..057f79f2 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -59,15 +59,23 @@ namespace reach { } + 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::getSharedRobotModelLoader(node_, "robot_description")->getModel(); + RCLCPP_INFO(LOGGER, "Created robot model!!!"); - ps_pub_ = node_->create_publisher("pose_stamped", 1); + ps_pub_ = node_->create_publisher("pose_stamped", 1); - try + try { ik_solver_ = solver_loader_.createSharedInstance(sp_.ik_solver_config_name); display_ = display_loader_.createSharedInstance(sp_.display_config_name); @@ -75,19 +83,25 @@ namespace reach catch (const pluginlib::PluginlibException &ex) { 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 std::exception &ex) { 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_name, node_) || - !display_->initialize(sp_.display_config_name, node_)) + 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!"); + RCLCPP_ERROR(LOGGER, "Could not initialized both display and ik solver plugins!"); + ik_solver_.reset(); + display_.reset(); return false; } @@ -122,7 +136,7 @@ namespace reach sp_ = sp; // Initialize the study - if (!initializeStudy()) + if (!initializeStudy(sp)) { RCLCPP_ERROR(LOGGER, "Failed to initialize the reach study"); return false; @@ -132,6 +146,8 @@ namespace reach if (!getReachObjectPointCloud()) { RCLCPP_ERROR(LOGGER, "Unable to obtain reach object point cloud"); + ik_solver_.reset(); + display_.reset(); return false; } @@ -224,6 +240,9 @@ namespace reach } } + ik_solver_.reset(); + display_.reset(); + return true; } diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index 005f421d..d04deff9 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -34,7 +34,7 @@ namespace reach { } - bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr node) + bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr const node,std::shared_ptr model ) { try { @@ -60,7 +60,7 @@ namespace reach continue; } - if (!plugin->initialize(name, node)) + if (!plugin->initialize(name, node, model)) { RCLCPP_WARN_STREAM(LOGGER, "Plugin '" << name << "' failed to be initialized; excluding it from the list"); continue; diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index d75d95f7..1aa59709 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -42,6 +42,7 @@ class RobotReachStudyNode : public rclcpp::Node !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("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) || diff --git a/reach_demo/config/params.yaml b/reach_demo/config/params.yaml index d2c1b5fd..d170a72b 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -10,6 +10,7 @@ robot_reach_study_node: get_avg_neighbor_count: false compare_dbs: [""] visualize_results: true + planning_group: "manipulator" optimization: radius: 0.2 From d98c95539cb154de244945ad76d67a09f9532854 Mon Sep 17 00:00:00 2001 From: Lovro Date: Mon, 14 Feb 2022 16:11:48 +0100 Subject: [PATCH 24/29] Remove sever warning caused by robot model loader. --- .../moveit_reach_plugins/display/moveit_reach_display.h | 4 ++-- .../moveit_reach_plugins/evaluation/distance_penalty_moveit.h | 4 ++-- .../moveit_reach_plugins/evaluation/joint_penalty_moveit.h | 4 ++-- .../moveit_reach_plugins/evaluation/manipulability_moveit.h | 4 ++-- .../moveit_reach_plugins/ik/discretized_moveit_ik_solver.h | 2 +- .../include/moveit_reach_plugins/ik/moveit_ik_solver.h | 4 ++-- moveit_reach_plugins/src/display/moveit_reach_display.cpp | 2 +- .../src/evaluation/distance_penalty_moveit.cpp | 2 +- moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp | 2 +- moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp | 2 +- moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp | 2 +- moveit_reach_plugins/src/ik/moveit_ik_solver.cpp | 2 +- reach_core/include/reach_core/plugins/evaluation_base.h | 2 +- reach_core/include/reach_core/plugins/ik_solver_base.h | 2 +- .../include/reach_core/plugins/impl/multiplicative_factory.h | 2 +- reach_core/include/reach_core/plugins/reach_display_base.h | 2 +- reach_core/include/reach_core/reach_study.h | 2 +- reach_core/src/core/reach_study.cpp | 3 +-- reach_core/src/plugins/impl/multiplicative_factory.cpp | 2 +- 19 files changed, 24 insertions(+), 25 deletions(-) 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 8f1d004b..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 @@ -42,7 +42,7 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase MoveItReachDisplay(); - bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; + bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual void showEnvironment() override; @@ -52,7 +52,7 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase private: - moveit::core::RobotModelPtr model_; + moveit::core::RobotModelConstPtr model_; planning_scene::PlanningScenePtr scene_; 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 f20d7227..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 @@ -40,13 +40,13 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase DistancePenaltyMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node,const std::shared_ptr model) 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; private: - moveit::core::RobotModelPtr model_; + moveit::core::RobotModelConstPtr model_; const moveit::core::JointModelGroup* jmg_; 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 f4959f51..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 @@ -34,7 +34,7 @@ class JointPenaltyMoveIt : public reach::plugins::EvaluationBase JointPenaltyMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) 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; @@ -42,7 +42,7 @@ class JointPenaltyMoveIt : public reach::plugins::EvaluationBase std::vector> getJointLimits(); - moveit::core::RobotModelPtr model_; + moveit::core::RobotModelConstPtr model_; const moveit::core::JointModelGroup* jmg_; 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 cd230e0a..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 @@ -33,13 +33,13 @@ class ManipulabilityMoveIt : public reach::plugins::EvaluationBase ManipulabilityMoveIt(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) 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; private: - moveit::core::RobotModelPtr model_; + moveit::core::RobotModelConstPtr model_; const moveit::core::JointModelGroup* jmg_; }; 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 14b75661..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 @@ -30,7 +30,7 @@ class DiscretizedMoveItIKSolver : public MoveItIKSolver DiscretizedMoveItIKSolver(); - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map& seed, 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 3e7c9c7e..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 @@ -59,7 +59,7 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase eval_.reset(); } - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; + virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) override; virtual std::optional solveIKFromSeed(const Eigen::Isometry3d& target, const std::map &seed, @@ -73,7 +73,7 @@ class MoveItIKSolver : public reach::plugins::IKSolverBase const moveit::core::JointModelGroup* jmg, const double* ik_solution) const; - moveit::core::RobotModelPtr model_; + moveit::core::RobotModelConstPtr model_; planning_scene::PlanningScenePtr scene_; const moveit::core::JointModelGroup* jmg_; diff --git a/moveit_reach_plugins/src/display/moveit_reach_display.cpp b/moveit_reach_plugins/src/display/moveit_reach_display.cpp index fd5a4edc..cbdba163 100644 --- a/moveit_reach_plugins/src/display/moveit_reach_display.cpp +++ b/moveit_reach_plugins/src/display/moveit_reach_display.cpp @@ -36,7 +36,7 @@ MoveItReachDisplay::MoveItReachDisplay() } -bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) +bool MoveItReachDisplay::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { RCLCPP_INFO(LOGGER, "Initializing MoveItReachDisplay!"); if (!reach::plugins::DisplayBase::initialize(name, node, model)) diff --git a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp index b8f76e63..41899c60 100644 --- a/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/distance_penalty_moveit.cpp @@ -30,7 +30,7 @@ DistancePenaltyMoveIt::DistancePenaltyMoveIt() } -bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) +bool DistancePenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { std::string planning_group; diff --git a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp index 5ccac052..973a1d91 100644 --- a/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/joint_penalty_moveit.cpp @@ -29,7 +29,7 @@ JointPenaltyMoveIt::JointPenaltyMoveIt() } -bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) +bool JointPenaltyMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { std::string planning_group; diff --git a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp index ec43421d..53a501fe 100644 --- a/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp +++ b/moveit_reach_plugins/src/evaluation/manipulability_moveit.cpp @@ -29,7 +29,7 @@ ManipulabilityMoveIt::ManipulabilityMoveIt() } -bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) +bool ManipulabilityMoveIt::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { std::string planning_group; 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 1a4d1602..f7870d4b 100644 --- a/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/discretized_moveit_ik_solver.cpp @@ -40,7 +40,7 @@ DiscretizedMoveItIKSolver::DiscretizedMoveItIKSolver() } -bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) +bool DiscretizedMoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { if(!MoveItIKSolver::initialize(name, node, model)) { diff --git a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp index 86ef4369..3e105d73 100644 --- a/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp +++ b/moveit_reach_plugins/src/ik/moveit_ik_solver.cpp @@ -34,7 +34,7 @@ MoveItIKSolver::MoveItIKSolver() } -bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) +bool MoveItIKSolver::initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) { node_ = node; diff --git a/reach_core/include/reach_core/plugins/evaluation_base.h b/reach_core/include/reach_core/plugins/evaluation_base.h index a1752525..aa7f5110 100644 --- a/reach_core/include/reach_core/plugins/evaluation_base.h +++ b/reach_core/include/reach_core/plugins/evaluation_base.h @@ -47,7 +47,7 @@ namespace reach * @brief initialize * @param config */ - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) = 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 017eeb00..bdd5a720 100644 --- a/reach_core/include/reach_core/plugins/ik_solver_base.h +++ b/reach_core/include/reach_core/plugins/ik_solver_base.h @@ -48,7 +48,7 @@ namespace reach * @param config * @return */ - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model) = 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. 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 25f773de..0842c17d 100644 --- a/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h +++ b/reach_core/include/reach_core/plugins/impl/multiplicative_factory.h @@ -35,7 +35,7 @@ namespace reach } } - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, std::shared_ptr model) override; + 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 e655ff0d..e73ee066 100644 --- a/reach_core/include/reach_core/plugins/reach_display_base.h +++ b/reach_core/include/reach_core/plugins/reach_display_base.h @@ -55,7 +55,7 @@ namespace reach marker_pub_.reset(); } - virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr model){ + 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 --git a/reach_core/include/reach_core/reach_study.h b/reach_core/include/reach_core/reach_study.h index 24f4aa5f..4c5bbb97 100644 --- a/reach_core/include/reach_core/reach_study.h +++ b/reach_core/include/reach_core/reach_study.h @@ -115,7 +115,7 @@ namespace reach rclcpp::Publisher::SharedPtr ps_pub_; // robot model - moveit::core::RobotModelPtr model_; + moveit::core::RobotModelConstPtr model_; }; diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 057f79f2..34949833 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -70,8 +70,7 @@ namespace reach ik_solver_.reset(); display_.reset(); // create robot model shared ptr - model_ = moveit::planning_interface::getSharedRobotModelLoader(node_, "robot_description")->getModel(); - RCLCPP_INFO(LOGGER, "Created robot model!!!"); + model_ = moveit::planning_interface::getSharedRobotModel(node_, "robot_description"); ps_pub_ = node_->create_publisher("pose_stamped", 1); diff --git a/reach_core/src/plugins/impl/multiplicative_factory.cpp b/reach_core/src/plugins/impl/multiplicative_factory.cpp index d04deff9..c4844ee9 100644 --- a/reach_core/src/plugins/impl/multiplicative_factory.cpp +++ b/reach_core/src/plugins/impl/multiplicative_factory.cpp @@ -34,7 +34,7 @@ namespace reach { } - bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr const node,std::shared_ptr model ) + bool MultiplicativeFactory::initialize(std::string& name, rclcpp::Node::SharedPtr const node,std::shared_ptr model ) { try { From b57c963d080ec28dd02f245b9b89820c73cc2900 Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 15 Feb 2022 10:46:24 +0100 Subject: [PATCH 25/29] Specific printing in data loader. --- reach_core/src/core/reach_study.cpp | 3 + reach_core/src/data_loader_node.cpp | 237 ++++++++++++++++++---------- 2 files changed, 160 insertions(+), 80 deletions(-) diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index 34949833..a0c49caf 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -175,6 +175,8 @@ namespace reach runInitialReachStudy(); db_->printResults(); visualizer_->update(); + // li added tmp + return true; } else { @@ -184,6 +186,7 @@ namespace reach db_->printResults(); visualizer_->update(); + } // Create an efficient search tree for doing nearest neighbors search diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index 18e84057..25d0b7e5 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -24,6 +24,11 @@ const static std::string RESULTS_FOLDER_NAME = "results"; const static std::string OPT_DB_NAME = "optimized_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 std::filesystem::path& root, const std::string& ext, std::vector>& ret) @@ -43,64 +48,76 @@ bool get_all(const std::filesystem::path& root, std::pair tmp; tmp.first = it->path().parent_path().filename(); tmp.second = it->path(); + //###########3 +// { +// if (tmp.first.string().find("(") != std::string::npos) { +// size_t idx_start = tmp.first.string().find('(') + 1; +// size_t idx_end = tmp.first.string().find(')') - 1; +// std::string config_name = tmp.first.string().substr(0, idx_start - 2); +// std::string coordinates = tmp.first.string().substr(idx_start, idx_end); +// +// std::stringstream ss(coordinates); +// std::vector v; +// +// while (ss.good()) { +// std::string substr; +// getline(ss, substr, ','); +// v.push_back(std::stod(substr)); +// } +// +// name_map[config_name].push_back(std::make_pair(tmp.first ,std::make_pair(tmp.second, std::make_pair(v[0], v[1])))); +// std::cout << config_name << std::endl; +// for (size_t i = 0; i < v.size(); i++) +// std::cout << v[i] << std::endl; +// } +// } + // ################## + ret.push_back(tmp); } } ++it; } - std::sort(ret.begin(), ret.end(), - [&](const std::pair &first, - std::pair &second){ +// for(auto &name: name_map) { +// std::sort(name.second.begin(), name.second.end(), +// [&](coordinate_config &first, +// coordinate_config &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; +// +// }); +// } - 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; + 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; } -//bool get_all_subdirs(const std::filesystem::path& root, -// const std::string& ext, -// std::vector>& ret) -//{ -// if(!std::filesystem::exists(root)) return false; -// -// if(!std::filesystem::is_directory(root)) return false; -// -// std::filesystem::recursive_directory_iterator it(root); -// std::filesystem::recursive_directory_iterator endit; -// -// while(it != endit) -// { -// printf("%s\n", it->path().filename().c_str()); -// 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; -// tmp.first = it->path().parent_path().filename(); -// tmp.second = it->path(); -// ret.push_back(tmp); -// } -// } -// ++it; -// } -// -// std::sort(ret.begin(), ret.end()); -// -// return true; -//} int main(int argc, char **argv) { @@ -138,44 +155,104 @@ int main(int argc, char **argv) return 0; } - if (avg_neighbor_count) { - std::cout << boost::format("%-30s %=25s %=25s %=25s %=25s\n") - % "Configuration Name" - % "Reach Percentage" - % "Normalized Total Pose Score" - % "Average Reachable Neighbors" - % "Average Joint Distance"; - }else { - std::cout << boost::format("%-30s %=25s %=25s\n") - % "Configuration Name" - % "Reach Percentage" - % "Normalized Total Pose Score"; + //## + { + 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); + + 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))); + } + + } + + 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("%-30s %=25s %=25s\n") + % "Configuration Name" + % "Reach Percentage" + % "Normalized Total Pose Score"; + std::cout << boost::format("%-30s\n") + % it->first; + for (auto &iter: it->second) { + std::cout << boost::format("%-30s %=25.3f %=25.6f\n") + % iter.second.first + % iter.second.second.reach_percentage + % iter.second.second.norm_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("%-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; - }else { - std::cout << boost::format("%-30s %=25.3f %=25.6f\n") - % config.c_str() - % res.reach_percentage - % res.norm_total_pose_score; - } + bool print_result_total = false; + if(print_result_total) { + if (avg_neighbor_count) { + std::cout << boost::format("%-30s %=25s %=25s %=25s %=25s\n") + % "Configuration Name" + % "Reach Percentage" + % "Normalized Total Pose Score" + % "Average Reachable Neighbors" + % "Average Joint Distance"; + } else { + std::cout << boost::format("%-30s %=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("%-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; + } else { + std::cout << boost::format("%-30s %=25.3f %=25.6f\n") + % config.c_str() + % res.reach_percentage + % res.norm_total_pose_score; + } + } + } } - } + // shutdown rclcpp::shutdown(); From 0d717ecd8902da0b82ec8d4164a76dd87d6a81ee Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 15 Feb 2022 12:09:55 +0100 Subject: [PATCH 26/29] Add special printing. --- reach_core/src/data_loader_node.cpp | 116 ++++++++++++++++------------ 1 file changed, 67 insertions(+), 49 deletions(-) diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index 25d0b7e5..645c43d2 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -22,7 +22,8 @@ #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; @@ -48,55 +49,12 @@ bool get_all(const std::filesystem::path& root, std::pair tmp; tmp.first = it->path().parent_path().filename(); tmp.second = it->path(); - //###########3 -// { -// if (tmp.first.string().find("(") != std::string::npos) { -// size_t idx_start = tmp.first.string().find('(') + 1; -// size_t idx_end = tmp.first.string().find(')') - 1; -// std::string config_name = tmp.first.string().substr(0, idx_start - 2); -// std::string coordinates = tmp.first.string().substr(idx_start, idx_end); -// -// std::stringstream ss(coordinates); -// std::vector v; -// -// while (ss.good()) { -// std::string substr; -// getline(ss, substr, ','); -// v.push_back(std::stod(substr)); -// } -// -// name_map[config_name].push_back(std::make_pair(tmp.first ,std::make_pair(tmp.second, std::make_pair(v[0], v[1])))); -// std::cout << config_name << std::endl; -// for (size_t i = 0; i < v.size(); i++) -// std::cout << v[i] << std::endl; -// } -// } - // ################## - ret.push_back(tmp); } } ++it; } -// for(auto &name: name_map) { -// std::sort(name.second.begin(), name.second.end(), -// [&](coordinate_config &first, -// coordinate_config &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; -// -// }); -// } std::sort(ret.begin(), ret.end(), [&](const std::pair &first, @@ -131,11 +89,17 @@ int main(int argc, char **argv) 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 = std::string(ament_index_cpp::get_package_share_directory(pkg_name)) + "/" + dir_name; @@ -155,8 +119,8 @@ int main(int argc, char **argv) return 0; } - //## - { + // 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(); @@ -195,11 +159,15 @@ int main(int argc, char **argv) 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("%-30s %=25.3f %=25.6f\n") @@ -210,9 +178,59 @@ int main(int argc, char **argv) } } - //## + 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; + } + } - bool print_result_total = false; if(print_result_total) { if (avg_neighbor_count) { std::cout << boost::format("%-30s %=25s %=25s %=25s %=25s\n") From a8626ecad5617d394a256911ee46b12952120f68 Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 15 Feb 2022 12:21:19 +0100 Subject: [PATCH 27/29] Add param to run initial study only. --- reach_core/include/reach_core/study_parameters.h | 1 + reach_core/src/core/reach_study.cpp | 10 ++++++++-- reach_core/src/robot_reach_study_node.cpp | 1 + reach_demo/config/params.yaml | 1 + 4 files changed, 11 insertions(+), 2 deletions(-) diff --git a/reach_core/include/reach_core/study_parameters.h b/reach_core/include/reach_core/study_parameters.h index d369fdac..c3caf73c 100644 --- a/reach_core/include/reach_core/study_parameters.h +++ b/reach_core/include/reach_core/study_parameters.h @@ -65,6 +65,7 @@ struct StudyParameters std::string fixed_frame; std::string object_frame; std::string planning_group; + bool run_initial_study_only; }; } // namespace core diff --git a/reach_core/src/core/reach_study.cpp b/reach_core/src/core/reach_study.cpp index a0c49caf..28473604 100644 --- a/reach_core/src/core/reach_study.cpp +++ b/reach_core/src/core/reach_study.cpp @@ -175,8 +175,10 @@ namespace reach runInitialReachStudy(); db_->printResults(); visualizer_->update(); - // li added tmp - return true; + // check if we don't have to optimize + if (sp.run_initial_study_only) { + return true; + } } else { @@ -186,6 +188,10 @@ namespace reach db_->printResults(); visualizer_->update(); + // check if we don't have to optimize + if (sp.run_initial_study_only) { + return true; + } } diff --git a/reach_core/src/robot_reach_study_node.cpp b/reach_core/src/robot_reach_study_node.cpp index 1aa59709..2593f65b 100644 --- a/reach_core/src/robot_reach_study_node.cpp +++ b/reach_core/src/robot_reach_study_node.cpp @@ -43,6 +43,7 @@ class RobotReachStudyNode : public rclcpp::Node !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) || diff --git a/reach_demo/config/params.yaml b/reach_demo/config/params.yaml index d170a72b..89bf1ff6 100644 --- a/reach_demo/config/params.yaml +++ b/reach_demo/config/params.yaml @@ -11,6 +11,7 @@ robot_reach_study_node: compare_dbs: [""] visualize_results: true planning_group: "manipulator" + run_initial_study_only: false optimization: radius: 0.2 From a9c9a425bc298f3c947be249d5e1297e007b38ca Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 15 Feb 2022 13:21:14 +0100 Subject: [PATCH 28/29] Extend printing format. --- reach_core/src/data_loader_node.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/reach_core/src/data_loader_node.cpp b/reach_core/src/data_loader_node.cpp index 645c43d2..45402aee 100644 --- a/reach_core/src/data_loader_node.cpp +++ b/reach_core/src/data_loader_node.cpp @@ -161,16 +161,16 @@ int main(int argc, char **argv) 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("%-30s %=25.3f %=25.6f\n") + 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; @@ -217,7 +217,7 @@ int main(int argc, char **argv) std::cout << boost::format("----------------------------------------------------------------------------\n")<first % it->second; } @@ -233,14 +233,14 @@ int main(int argc, char **argv) if(print_result_total) { if (avg_neighbor_count) { - std::cout << boost::format("%-30s %=25s %=25s %=25s %=25s\n") + 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("%-30s %=25s %=25s\n") + std::cout << boost::format("%-60s %=25s %=25s\n") % "Configuration Name" % "Reach Percentage" % "Normalized Total Pose Score"; @@ -255,14 +255,14 @@ int main(int argc, char **argv) if (db.load(path)) { reach::core::StudyResults res = db.getStudyResults(); if (avg_neighbor_count) { - std::cout << boost::format("%-30s %=25.3f %=25.6f %=25.3f %=25.3f\n") + 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("%-30s %=25.3f %=25.6f\n") + std::cout << boost::format("%-60s %=25.3f %=25.6f\n") % config.c_str() % res.reach_percentage % res.norm_total_pose_score; From 47fc529e0e257f139ce7744af541431ec94ba768 Mon Sep 17 00:00:00 2001 From: Lovro Date: Tue, 15 Feb 2022 16:04:47 +0100 Subject: [PATCH 29/29] Fix deps. --- reach_core/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/reach_core/package.xml b/reach_core/package.xml index 0ca8bc51..b455b30a 100644 --- a/reach_core/package.xml +++ b/reach_core/package.xml @@ -26,6 +26,7 @@ tf2_ros tf2_eigen visualization_msgs + moveit_ros_planning_interface