diff --git a/rasms_manipulator/config/params.yaml b/rasms_manipulator/config/params.yaml index a3f647c..c1c1558 100644 --- a/rasms_manipulator/config/params.yaml +++ b/rasms_manipulator/config/params.yaml @@ -5,4 +5,4 @@ move: waypoints: ["one", "two"] waypoint_coords: one: [0.0, 0.0, 0.0] - two: [0.28, -0.2, 0.5] + two: [0.01, 0.01, 0.6] diff --git a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp index 5509d2d..604dffb 100644 --- a/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp +++ b/rasms_manipulator/include/rasms_bt/behavior_tree_nodes/Move.hpp @@ -13,7 +13,7 @@ namespace rasms_manipulation { - class Move : public BT::AsyncActionNode + class Move : public BT::ActionNodeBase { public: Move(const std::string &xml_tag_name, @@ -32,6 +32,7 @@ namespace rasms_manipulation private: std::map waypoints_; std::atomic_bool _halt_requested; + int counter_; }; } // namespace rasms_manipulation \ No newline at end of file diff --git a/rasms_manipulator/launch/rasms_manipulation.launch.py b/rasms_manipulator/launch/rasms_manipulation.launch.py index c3e61d5..74a0e9e 100644 --- a/rasms_manipulator/launch/rasms_manipulation.launch.py +++ b/rasms_manipulator/launch/rasms_manipulation.launch.py @@ -1,16 +1,55 @@ import os +import yaml from ament_index_python.packages import get_package_share_directory - from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +def load_file(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, "r") as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +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, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + def generate_launch_description(): + # planning_context + robot_description_config = load_file( + "rasms_description", "urdf/rasms_description.urdf.xacro" + ) + robot_description = {"robot_description": robot_description_config} + + robot_description_semantic_config = load_file( + "rasms_moveit_config", "srdf/rasms_description.srdf" + ) + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + "rasms_moveit_config", "config/kinematics.yml" + ) + pkg_dir = get_package_share_directory('rasms_manipulator') namespace = LaunchConfiguration('namespace') @@ -40,16 +79,21 @@ def generate_launch_description(): output='screen', parameters=[ pkg_dir + '/config/params.yaml', + robot_description, + robot_description_semantic, + kinematics_yaml, { 'action_name': 'move', - ### [Groot](https://github.com/BehaviorTree/Groot) behavior_tree_visual tool - ### [](https://intelligentroboticslab.gsyc.urjc.es/ros2_planning_system.github.io/design/index.html) - # need to provide visual information about working behavior tree - # 'publisher_port': 1668, - # 'server_port': 1669, 'bt_xml_file': pkg_dir + '/behavior_trees_xml/move.xml' } ]) + transport_1_cmd = Node( + package='rasms_manipulator', + executable='rasms_action_node', + name='transport_1', + namespace=namespace, + output='screen', + parameters=[]) # Create the launch description and populate ld = LaunchDescription() @@ -59,5 +103,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(plansys2_cmd) ld.add_action(move_1) + # ld.add_action(transport_1_cmd) return ld \ No newline at end of file diff --git a/rasms_manipulator/pddl/commands b/rasms_manipulator/pddl/commands index dd5cd38..60e929b 100644 --- a/rasms_manipulator/pddl/commands +++ b/rasms_manipulator/pddl/commands @@ -1,6 +1,6 @@ set instance rasms robot set instance one zone +set instance two zone set predicate (robot_at rasms one) -set goal (and(robot_moved rasms one)) +set goal (and(robot_at rasms two)) run - diff --git a/rasms_manipulator/src/behavior_tree_nodes/Move.cpp b/rasms_manipulator/src/behavior_tree_nodes/Move.cpp index 4dcc520..e374db0 100644 --- a/rasms_manipulator/src/behavior_tree_nodes/Move.cpp +++ b/rasms_manipulator/src/behavior_tree_nodes/Move.cpp @@ -14,9 +14,11 @@ namespace rasms_manipulation { + static const rclcpp::Logger LOGGER = rclcpp::get_logger("plansys2_move_node"); + Move::Move(const std::string &xml_tag_name, const BT::NodeConfiguration &conf) - : AsyncActionNode(xml_tag_name, conf) + : ActionNodeBase(xml_tag_name, conf), counter_(0) { rclcpp::Node::SharedPtr node; config().blackboard->get("node", node); @@ -28,7 +30,7 @@ namespace rasms_manipulation } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex) { - std::cerr << ex.what() << '\n'; + RCLCPP_WARN(LOGGER, "%c", ex.what()); } if (node->has_parameter("waypoints")) @@ -56,7 +58,7 @@ namespace rasms_manipulation } else { - std::cerr << "No coordinate configured for waypoint [" << wp << "]" << std::endl; + RCLCPP_WARN(LOGGER, "No coordinate configured for waypoint [%s]", wp.c_str()); } } } @@ -65,17 +67,21 @@ namespace rasms_manipulation void Move::halt() { _halt_requested = true; - std::cout << "Move halt" << std::endl; + RCLCPP_INFO(LOGGER, "Move halt"); } BT::NodeStatus Move::tick() { - rclcpp::Node::SharedPtr node; - config().blackboard->get("node", node); - // rclcpp::executors::SingleThreadedExecutor executor; - // executor.add_node(node); - // std::thread([&executor]() {executor.spin();}).detach(); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_node = rclcpp::Node::make_shared("move_group_interface_tutorial", node_options); + + config().blackboard->get("node", move_group_node); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(move_group_node); + std::thread([&executor]() { executor.spin(); }).detach(); std::string goal; getInput("goal", goal); @@ -84,53 +90,46 @@ namespace rasms_manipulation if (waypoints_.find(goal) != waypoints_.end()) { pose2moveit = waypoints_[goal]; - std::cout << pose2moveit.position.x << ", " << pose2moveit.position.y << ", " << pose2moveit.position.z << std::endl; + RCLCPP_INFO(LOGGER, "Read goal [%f, %f, %f]", pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z); + } else { + RCLCPP_WARN(LOGGER, "No coordinate for waypoint [%s]", goal.c_str()); } - else - { - std::cerr << "No coordinate for waypoint [" << goal << "]" << std::endl; - } - //Setup MoveIt interface for planning - moveit::planning_interface::MoveGroupInterface::Options move_group_options("rasms_arm_group", "/robot_description"); - moveit::planning_interface::MoveGroupInterface move_group_interface(node, move_group_options); - - // Raw pointers are frequently used to refer to the planning group for improved performance. - // const moveit::core::JointModelGroup* joint_model_group = - // move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP); - - // move_group_interface->setStartState(*move_group_interface->getCurrentState()); + static const std::string PLANNING_GROUP = "rasms_arm_group"; + RCLCPP_INFO(LOGGER, "MoveGroupInterface set current planning group `%s`", PLANNING_GROUP.c_str()); + + moveit::planning_interface::MoveGroupInterface::Options move_group_options(PLANNING_GROUP, "robot_description"); + moveit::planning_interface::MoveGroupInterface move_group_interface(move_group_node, move_group_options); + + moveit::core::RobotState start_state(*move_group_interface.getCurrentState()); + move_group_interface.setStartState(start_state); - /*---------- - 00823 // pose goal; - 00824 // for each link we have a set of possible goal locations; - 00825 std::map > pose_targets_; - ----------*/ geometry_msgs::msg::Pose goal_pos; goal_pos.orientation.w = 1.0; goal_pos.position.x = pose2moveit.position.x; goal_pos.position.y = pose2moveit.position.y; goal_pos.position.z = pose2moveit.position.z; - std::cout << "[MOVE_STARTED]: goal [" << - goal_pos.position.x << ", " << - goal_pos.position.y << ", " << - goal_pos.position.z << ", " << - "]" << std::endl; + + RCLCPP_INFO(LOGGER, "Move started to goal [%f, %f, %f]", goal_pos.position.x, goal_pos.position.y, goal_pos.position.z); move_group_interface.setPoseTarget(goal_pos); moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); if (success) { - //RCLCPP_INFO("Planning successful"); - // move_group_interface.setStartStateToCurrentState(); + RCLCPP_INFO(LOGGER, "Planning success"); move_group_interface.execute(my_plan); move_group_interface.move(); + counter_++; } else { - std::cout << " failed to generate a plan" << std::endl; + + RCLCPP_WARN(LOGGER, "Failed to generate a plan"); return BT::NodeStatus::FAILURE; } + if (counter_ != waypoints_.size()) + return BT::NodeStatus::RUNNING; + return BT::NodeStatus::SUCCESS; } diff --git a/rasms_manipulator/src/rasms_controller_node.cpp b/rasms_manipulator/src/rasms_controller_node.cpp index 7cce0f3..ea9d5f4 100644 --- a/rasms_manipulator/src/rasms_controller_node.cpp +++ b/rasms_manipulator/src/rasms_controller_node.cpp @@ -12,15 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include -#include #include "plansys2_msgs/msg/action_execution_info.hpp" +#include "plansys2_msgs/msg/plan.hpp" -#include "plansys2_executor/ExecutorClient.hpp" #include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_executor/ExecutorClient.hpp" #include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -36,9 +38,9 @@ public: void init() { domain_expert_ = std::make_shared(); + planner_client_ = std::make_shared(); problem_expert_ = std::make_shared(); executor_client_ = std::make_shared(); - planner_client_ = std::make_shared(); init_knowledge(); diff --git a/rasms_moveit_config/launch/rasms_control.launch.py b/rasms_moveit_config/launch/rasms_control.launch.py index 9418157..3003009 100644 --- a/rasms_moveit_config/launch/rasms_control.launch.py +++ b/rasms_moveit_config/launch/rasms_control.launch.py @@ -86,7 +86,7 @@ def generate_launch_description(): arguments=["rasms_arm_controller", "--controller-manager", "/controller_manager"], ) - """load_controllers = [] + load_controllers = [] for controller in [ "rasms_arm_controller", "joint_state_broadcaster", @@ -97,17 +97,17 @@ def generate_launch_description(): shell=True, output="screen", ) - ]""" + ] return LaunchDescription( launch_args + - #load_controllers + + load_controllers + [ controller_manager, robot_state_publisher, - joint_state_publisher, static_tf, joint_state_broadcaster, - rasms_arm_controller + rasms_arm_controller, + joint_state_publisher ] )