From 2019e7db41dbdc78eb486a15b03ff8de81338cb5 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Sun, 5 Feb 2023 17:04:54 +0300 Subject: [PATCH] fix last merge --- rbs_bt_executor/bt_trees/test_tree.xml | 14 ++-- rbs_bt_executor/launch/rbs_executor.launch.py | 2 +- rbs_bt_executor/src/MoveToPose.cpp | 64 ++++++++++++++++--- .../launch/rbs_simulation.launch.py | 15 +---- sdf_models/CMakeLists.txt | 2 +- 5 files changed, 66 insertions(+), 31 deletions(-) diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml index 73d072c..6d75809 100644 --- a/rbs_bt_executor/bt_trees/test_tree.xml +++ b/rbs_bt_executor/bt_trees/test_tree.xml @@ -9,22 +9,22 @@ server_name="/move_topose" server_timeout="1000" cancel_timeout="500" /> - - + server_timeout="1000" + cancel_timeout="500" /> + + diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py index 5f222a3..70c60c0 100644 --- a/rbs_bt_executor/launch/rbs_executor.launch.py +++ b/rbs_bt_executor/launch/rbs_executor.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): # Do not declare a node name otherwise it messes with the action node names and will result in # duplicate nodes! parameters=[ - {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_queue'), 'bt_xmls/test_tree.xml')}, + {'bt_file_path': os.path.join(get_package_share_directory('rbs_bt_executor'), 'bt_trees/test_tree.xml')}, {'plugins': ['rbs_skill_move_topose_bt_action_client']}, points ] diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp index fc1f91d..9842e2b 100644 --- a/rbs_bt_executor/src/MoveToPose.cpp +++ b/rbs_bt_executor/src/MoveToPose.cpp @@ -17,25 +17,65 @@ class MoveToPose : public BtAction MoveToPose(const std::string & name, const BT::NodeConfiguration & config) : BtAction(name, config) { - //auto params = _node->declare_parameters("Poses") + if (!_node->has_parameter("pose1")) + { + _node->declare_parameter("pose1",std::vector{ 0.11724797630931184, + 0.46766635768602544, + 0.5793862343094913, + 0.9987999001314066, + 0.041553846820940925, + -0.004693314677006583, + -0.025495295825239704 + } + ); + } + + if (!_node->has_parameter("pose2")) + { + _node->declare_parameter("pose2",std::vector{ -0.11661364861606047, + 0.4492600889665261, + 0.5591700913807053, + 0.9962273179258467, + 0.04057380155886888, + 0.009225849745372298, + 0.07615629548377048 + } + ); + } + + if (!_node->has_parameter("pose3")) + { + _node->declare_parameter("pose3",std::vector{ -0.07133612047767886, + 0.41038906578748613, + 0.2844649846989331, + 0.999078481789772, + 0.04109234110437432, + 0.006539754292177074, + 0.010527978961032304 + } + ); + } RCLCPP_INFO(_node->get_logger(), "Start the node"); - pose_des.position.x = 0.11724797630931184; - pose_des.position.y = 0.46766635768602544; - pose_des.position.z = 0.5793862343094913; - pose_des.orientation.x = 0.9987999001314066; - pose_des.orientation.y = 0.041553846820940925; - pose_des.orientation.z = -0.004693314677006583; - pose_des.orientation.w = -0.025495295825239704; } MoveToPoseAction::Goal populate_goal() override { getInput("robot_name", robot_name_); getInput("pose", pose_); + + rclcpp::Parameter arr = _node->get_parameter(pose_); + std::vector possd = arr.as_double_array(); - //auto pose = _node->get_parameter(pose_).get_parameter_value().get(); + pose_des.position.x = possd[0]; + pose_des.position.y = possd[1]; + pose_des.position.z = possd[2]; + pose_des.orientation.x = possd[3]; + pose_des.orientation.y = possd[4]; + pose_des.orientation.z = possd[5]; + pose_des.orientation.w = possd[6]; + - RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f orientation.w: %f", + RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f", pose_des.position.x, pose_des.position.y, pose_des.position.z, pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w); @@ -66,6 +106,10 @@ class MoveToPose : public BtAction geometry_msgs::msg::Pose pose_des; std::map Poses; + // geometry_msgs::msg::Pose array_to_pose(std::vector pose_array){ + + // } + }; // class MoveToPose diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index 2d4033c..aa39122 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -3,12 +3,11 @@ from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, - OpaqueFunction, - RegisterEventHandler, + RegisterEventHandler ) from ament_index_python.packages import get_package_share_directory, get_package_share_path from launch.conditions import IfCondition, UnlessCondition -from launch.event_handlers import OnProcessExit +from launch.event_handlers import OnProcessExit, OnExecutionComplete from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node @@ -161,7 +160,6 @@ def generate_launch_description(): [FindPackageShare("sdf_models"), "worlds", "empty_world.sdf"] # TODO тут пакет извне должен задаваться ) - print(world_config_file) robot_description_content = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -262,9 +260,6 @@ def generate_launch_description(): [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] ) - # robot_description_planning = { - # "robot_description_planning": load_yaml_abs(str(joint_limit_params.perform(context))) - # } # Planning Configuration ompl_planning_pipeline_config = { @@ -314,7 +309,6 @@ def generate_launch_description(): robot_description, robot_description_semantic, robot_description_kinematics, - # robot_description_planning, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, @@ -334,7 +328,6 @@ def generate_launch_description(): robot_description_semantic, ompl_planning_pipeline_config, robot_description_kinematics, - # robot_description_planning, ], condition=IfCondition(launch_rviz), ) @@ -347,7 +340,6 @@ def generate_launch_description(): robot_description, robot_description_semantic, robot_description_kinematics, - # robot_description_planning, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, @@ -364,7 +356,6 @@ def generate_launch_description(): robot_description, robot_description_semantic, robot_description_kinematics, - # robot_description_planning, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, @@ -383,7 +374,7 @@ def generate_launch_description(): gazebo_spawn_robot, move_group_node, move_topose_action_server, - move_cartesian_path_action_server + move_cartesian_path_action_server, ] return LaunchDescription(declared_arguments + nodes_to_start) \ No newline at end of file diff --git a/sdf_models/CMakeLists.txt b/sdf_models/CMakeLists.txt index 01efca4..80fc1d9 100644 --- a/sdf_models/CMakeLists.txt +++ b/sdf_models/CMakeLists.txt @@ -29,6 +29,6 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -install(DIRECTORY models DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY models worlds DESTINATION share/${PROJECT_NAME}) ament_package()