From af973fe0ee018aa793a21a49bbfc1a04bcd68b30 Mon Sep 17 00:00:00 2001 From: Splinter1984 Date: Wed, 17 Nov 2021 23:01:23 +0700 Subject: [PATCH] #6: try to build node --- .../config => config}/joint_limits.yml | 10 +- .../config => config}/kinematics.yml | 0 .../config => config}/moveit_cpp.yml | 0 .../config => config}/ompl_planning.yml | 0 .../config => config}/rasms.rviz | 0 .../config => config}/rasms_controllers.yml | 0 .../rasms_moveit_controllers.yml | 0 .../config => config}/robasm_sgonov.rviz | 0 rasms_description/urdf/rasms.gazebo.xacro | 16 +-- .../.vscode/c_cpp_properties.json | 22 ++++ rasms_manipulator/.vscode/settings.json | 14 +++ rasms_manipulator/behavior_trees/move.xml | 7 ++ rasms_manipulator/config/params.yaml | 9 ++ .../rasms_bt/behavior_tree_nodes/Move.hpp | 64 ++++++++++ .../src/behavior_tree_nodes/Move.cpp | 115 ++++++++++++++++++ .../src/rasms_controller_node.cpp | 110 +++++++++++++++++ 16 files changed, 355 insertions(+), 12 deletions(-) rename {rasms_moveit_config/config => config}/joint_limits.yml (91%) rename {rasms_moveit_config/config => config}/kinematics.yml (100%) rename {rasms_moveit_config/config => config}/moveit_cpp.yml (100%) rename {rasms_moveit_config/config => config}/ompl_planning.yml (100%) rename {rasms_moveit_config/config => config}/rasms.rviz (100%) rename {rasms_moveit_config/config => config}/rasms_controllers.yml (100%) rename {rasms_moveit_config/config => config}/rasms_moveit_controllers.yml (100%) rename {rasms_moveit_config/config => config}/robasm_sgonov.rviz (100%) create mode 100644 rasms_manipulator/.vscode/c_cpp_properties.json create mode 100644 rasms_manipulator/.vscode/settings.json create mode 100644 rasms_manipulator/behavior_trees/move.xml create mode 100644 rasms_manipulator/config/params.yaml create mode 100644 rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp create mode 100644 rasms_manipulator/src/behavior_tree_nodes/Move.cpp create mode 100644 rasms_manipulator/src/rasms_controller_node.cpp diff --git a/rasms_moveit_config/config/joint_limits.yml b/config/joint_limits.yml similarity index 91% rename from rasms_moveit_config/config/joint_limits.yml rename to config/joint_limits.yml index 0115bd8..8227e0a 100644 --- a/rasms_moveit_config/config/joint_limits.yml +++ b/config/joint_limits.yml @@ -10,26 +10,26 @@ default_acceleration_scaling_factor: 0.1 joint_limits: joint1: has_velocity_limits: false - max_velocity: 0 + max_velocity: 0.0 has_acceleration_limits: false max_acceleration: 0 joint3: has_velocity_limits: false - max_velocity: 0 + max_velocity: 0.0 has_acceleration_limits: false max_acceleration: 0 joint4: has_velocity_limits: false - max_velocity: 0 + max_velocity: 0.0 has_acceleration_limits: false max_acceleration: 0 joint5: has_velocity_limits: false - max_velocity: 0 + max_velocity: 0.0 has_acceleration_limits: false max_acceleration: 0 joint6: has_velocity_limits: false - max_velocity: 0 + max_velocity: 0.0 has_acceleration_limits: false max_acceleration: 0 \ No newline at end of file diff --git a/rasms_moveit_config/config/kinematics.yml b/config/kinematics.yml similarity index 100% rename from rasms_moveit_config/config/kinematics.yml rename to config/kinematics.yml diff --git a/rasms_moveit_config/config/moveit_cpp.yml b/config/moveit_cpp.yml similarity index 100% rename from rasms_moveit_config/config/moveit_cpp.yml rename to config/moveit_cpp.yml diff --git a/rasms_moveit_config/config/ompl_planning.yml b/config/ompl_planning.yml similarity index 100% rename from rasms_moveit_config/config/ompl_planning.yml rename to config/ompl_planning.yml diff --git a/rasms_moveit_config/config/rasms.rviz b/config/rasms.rviz similarity index 100% rename from rasms_moveit_config/config/rasms.rviz rename to config/rasms.rviz diff --git a/rasms_moveit_config/config/rasms_controllers.yml b/config/rasms_controllers.yml similarity index 100% rename from rasms_moveit_config/config/rasms_controllers.yml rename to config/rasms_controllers.yml diff --git a/rasms_moveit_config/config/rasms_moveit_controllers.yml b/config/rasms_moveit_controllers.yml similarity index 100% rename from rasms_moveit_config/config/rasms_moveit_controllers.yml rename to config/rasms_moveit_controllers.yml diff --git a/rasms_moveit_config/config/robasm_sgonov.rviz b/config/robasm_sgonov.rviz similarity index 100% rename from rasms_moveit_config/config/robasm_sgonov.rviz rename to config/robasm_sgonov.rviz diff --git a/rasms_description/urdf/rasms.gazebo.xacro b/rasms_description/urdf/rasms.gazebo.xacro index b0a8efd..d65f7d5 100644 --- a/rasms_description/urdf/rasms.gazebo.xacro +++ b/rasms_description/urdf/rasms.gazebo.xacro @@ -1,6 +1,8 @@ + + @@ -15,7 +17,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable @@ -23,7 +25,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable @@ -31,7 +33,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable @@ -39,7 +41,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable @@ -47,7 +49,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable @@ -55,7 +57,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable @@ -63,7 +65,7 @@ 0.2 0.2 Gazebo/White - true + gravity_disable \ No newline at end of file diff --git a/rasms_manipulator/.vscode/c_cpp_properties.json b/rasms_manipulator/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..adac66b --- /dev/null +++ b/rasms_manipulator/.vscode/c_cpp_properties.json @@ -0,0 +1,22 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "", + "limitSymbolsToIncludedHeaders": true + }, + "includePath": [ + "/home/splinter1984/robas_ws/install/moveit_visual_tools/include/**", + "/home/splinter1984/robas_ws/install/rviz_visual_tools/include/**", + "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph/include/**", + "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/include/**", + "/opt/ros/foxy/include/**", + "/home/splinter1984/robas_ws/src/moveit_visual_tools/include/**", + "/home/splinter1984/robas_ws/src/rviz_visual_tools/include/**", + "/usr/include/**" + ], + "name": "ROS" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/rasms_manipulator/.vscode/settings.json b/rasms_manipulator/.vscode/settings.json new file mode 100644 index 0000000..7d24ba4 --- /dev/null +++ b/rasms_manipulator/.vscode/settings.json @@ -0,0 +1,14 @@ +{ + "cmake.configureOnOpen": true, + "python.autoComplete.extraPaths": [ + "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_viewer/lib/python3.8/site-packages", + "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/lib/python3.8/site-packages", + "/opt/ros/foxy/lib/python3.8/site-packages" + ], + "python.analysis.extraPaths": [ + "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_viewer/lib/python3.8/site-packages", + "/home/splinter1984/plansys_ws/install/ros2_knowledge_graph_msgs/lib/python3.8/site-packages", + "/opt/ros/foxy/lib/python3.8/site-packages" + ], + "ros.distro": "foxy" +} \ No newline at end of file diff --git a/rasms_manipulator/behavior_trees/move.xml b/rasms_manipulator/behavior_trees/move.xml new file mode 100644 index 0000000..f8a6dae --- /dev/null +++ b/rasms_manipulator/behavior_trees/move.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/rasms_manipulator/config/params.yaml b/rasms_manipulator/config/params.yaml new file mode 100644 index 0000000..5288e3c --- /dev/null +++ b/rasms_manipulator/config/params.yaml @@ -0,0 +1,9 @@ +move_1: + ros_parameters: + plugins: + - rasms_move_bt_node + waypoints: ["zero", "one"] + waypoints_coords: + zero: [0.0, -2.0, 0.0] + one: [1.8, 0.0, 0.0] + \ No newline at end of file diff --git a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp new file mode 100644 index 0000000..37b1a57 --- /dev/null +++ b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp @@ -0,0 +1,64 @@ +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose2_d.hpp" + +#include "plansys2_bt_actions/BTActionNode.hpp" +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "moveit_msgs/msg/place_location.hpp" +#include "moveit_msgs/msg/move_it_error_codes.hpp" + +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2/transform_datatypes.h" +#include "tf2/LinearMath/Transform.h" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" + +#include +#include + +#include + +namespace rasms_manipulation +{ +class Move: public BT::ActionNodeBase +{ +public: + explicit Move( + const std::string& xml_tag_name, + const BT::NodeConfiguration& conf); + + void halt(); + BT::NodeStatus on_tick(); + + geometry_msgs::msg::PoseStamped pose2BaseFootprint(geometry_msgs::msg::PoseStamped input); + void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg); + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("goal") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr move_pub_; + rclcpp::Subscription::SharedPtr result_sub_; + std::map places_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + int result_; + bool move_action_sent_; +}; + +} //namespace rasms_manipulator \ No newline at end of file diff --git a/rasms_manipulator/src/behavior_tree_nodes/Move.cpp b/rasms_manipulator/src/behavior_tree_nodes/Move.cpp new file mode 100644 index 0000000..8b23e4e --- /dev/null +++ b/rasms_manipulator/src/behavior_tree_nodes/Move.cpp @@ -0,0 +1,115 @@ +#include +#include + +#include "rasms_bt/behavior_tree_nodes/Move.hpp" + +#include "behaviortree_cpp_v3/behavior_tree.h" + +namespace rasms_manipulation +{ +Move::Move( + const std::string& xml_tag_name, + const BT::NodeConfiguration& conf) + : BT::ActionNodeBase(xml_tag_name, conf) +{ + node_ = rclcpp::Node::make_shared("move_action_comms"); + move_pub_ = node_->create_publisher("/moveit/place", 1); + result_sub_ = node_->create_subscription( + "/moveit/result", + 1, + std::bind(&Move::resultCallback, this, std::placeholders::_1)); + rclcpp::Node::SharedPtr bt_node; + config().blackboard->get("node", bt_node); + + tf_buffer_ = std::make_shared(node_->get_clock()); + auto timer_interface = std::make_shared( + node_->get_node_base_interface(), + node_->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + move_action_sent_ = false; +} + +void Move::halt() +{ + std::cout << "Move halt" << std::endl; +} + +void Move::resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg) +{ + result_ = msg->val; +} + +double getYaw(geometry_msgs::msg::Quaternion mQ) +{ + tf2::Quaternion tQ( + mQ.x, + mQ.y, + mQ.z, + mQ.w); + tf2::Matrix3x3 m(tQ); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + return yaw; +} + +geometry_msgs::msg::PoseStamped Move::pose2BaseFootprint(geometry_msgs::msg::PoseStamped input) +{ + tf2::Transform frame2bf, obj2frame, bf2Object; + obj2frame.setOrigin({input.pose.posetion.x, + input.pose.position.y, + input.pose.position.z}); + obj2frame.setRotation({input.pose.posetion.x, + input.pose.position.y, + input.pose.position.z, + input.pose.position.w}); + geometry_msgs::msg::PoseStamped object_pose; + try + { + auto tf = tf_buffer_->lookupTransform( + input.header.frame_id, "base", tf2::TimePointZero); + tf2::fromMsg(tf.transform, frame2bf); + bf2Object = obj2frame * frame2bf; + } +} + +BT::NodeStatus Move::on_tick() +{ + if (!move_action_sent_) + { + geometry_msgs::msg::PoseStamped goal; + getInput("goal", goal); + RCLCPP_INFO(node_->get_logger(), "Move to: x:%f y:%f z:%f yaw:%f", + goal.pose.position.x, + goal.pose.position.y, + goal.pose.position.z, + getYaw(goal.pose.orientation)); + moveit_msgs::msg::PlaceLocation msg; + msg.place_pose = pose2BaseFootprint(goal); + move_pub_->publish(msg); + result_ = 0; + move_action_sent_ = true; + } + + rclcpp:spin_some(node_); + + if (result_ == 0) + { + return BT::NodeStatus::RUNNING; + } else if (result_ == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + result_ = 0; + RCLCPP_INFO(node_->get_logger(), "Success!"); + return BT::NodeStatus::SUCCESS; + } else { + RCLCPP_ERROR(node_->get_logger(), "Move error: MoveItErrorCodes[%i]", result_); + return BT::NodeStatus::FAILURE; + } +} +} //namespace rasms_manipulator + +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("Move"); +} \ No newline at end of file diff --git a/rasms_manipulator/src/rasms_controller_node.cpp b/rasms_manipulator/src/rasms_controller_node.cpp new file mode 100644 index 0000000..24b6b13 --- /dev/null +++ b/rasms_manipulator/src/rasms_controller_node.cpp @@ -0,0 +1,110 @@ +#include +#include + +#include "plansys2_msgs/msg/action_execution_info.hpp" + +#include "plansys2_executor/ExecutorClient.hpp" +#include "plansys2_domain_expert/DomainExpertClient.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_planner/PlannerClient.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" + +class RasmsController: public rclcpp::Node +{ +public: + RasmsController(): rclcpp::Node("rasms_controller") + {} + + void init() + { + domain_expert_ = std::make_shared(); + problem_expert_ = std::make_shared(); + executor_client_ = std::make_shared(); + planner_client_ = std::make_shared(); + + init_knowledge(); + + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (plan.has_value()) + { + if (!executor_client_->start_plan_execution(plan.value())) + { + RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)"); + } + } else { + RCLCPP_ERROR_STREAM( + this->get_logger(), "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal())); + } + } + + void init_knowledge() + { + problem_expert_->addInstance(plansys2::Instance{"rasms", "robot"}); + problem_expert_->setGoal(plansys2::Goal("robot moved")); + } + + std::string status_to_string(int8_t status) + { + switch (status) + { + case plansys2_msgs::msg::ActionExecutionInfo::NOT_EXECUTED: + return "NOT_EXECUTED"; + break; + case plansys2_msgs::msg::ActionExecutionInfo::EXECUTING: + return "EXECUTING"; + break; + case plansys2_msgs::msg::ActionExecutionInfo::FAILED: + return "FAILED"; + break; + case plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED: + return "SUCCEEDED"; + break; + case plansys2_msgs::msg::ActionExecutionInfo::CANCELLED: + return "CANCELLED"; + break; + default: + return "UNKNOWN"; + break; + } + } + + void step() + { + if (!executor_client_->execute_and_check_plan()) + { + RCLCPP_INFO(get_logger(), "Done!"); + } + } + +private: + std::shared_ptr problem_expert_; + std::shared_ptr domain_expert_; + std::shared_ptr executor_client_; + std::shared_ptr planner_client_; +}; + +int main(argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + node->init(); + + rclcpp::Rate rate(1); + while (rclcpp::ok()) + { + rate.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + node->step(); + } + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file