fix last merge

rbs_bt_executor/bt_trees/test_tree.xml

@@ -9,22 +9,22 @@
                     server_name="/move_topose" 
                     server_timeout="1000"
                     cancel_timeout="500" />
-
+                    <Action ID="MoveToPose"  
-                    <!-- <Action ID="MoveToPose"  
                     robot_name="ur_manipulator" 
                     pose="pose2"
 
                     server_name="/move_topose" 
-                    server_timeout="10" 
+                    server_timeout="1000"
-                    cancel_timeout="5" />
+                    cancel_timeout="500" />
-
                     <Action ID="MoveToPose"  
                     robot_name="ur_manipulator" 
                     pose="pose3"
 
                     server_name="/move_topose" 
-                    server_timeout="10" 
+                    server_timeout="1000"
-                    cancel_timeout="5" /> -->
+                    cancel_timeout="500" />
+
+  
         </Sequence>
     </BehaviorTree>
 

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
             ]

rbs_bt_executor/src/MoveToPose.cpp

@@ -17,25 +17,65 @@ class MoveToPose : public BtAction<MoveToPoseAction>
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<MoveToPoseAction>(name, config) 
         {
-            //auto params = _node->declare_parameters("Poses")
+            if (!_node->has_parameter("pose1"))
+            {
+                _node->declare_parameter("pose1",std::vector<double>{   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<double>{   -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<double>{   -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<std::string>("robot_name", robot_name_);
             getInput<std::string>("pose", pose_);
+            
+            rclcpp::Parameter arr = _node->get_parameter(pose_);
+            std::vector<double> possd = arr.as_double_array();
 
-            //auto pose =  _node->get_parameter(pose_).get_parameter_value().get<geometry_msgs::msg::Pose>();
+            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<MoveToPoseAction>
         geometry_msgs::msg::Pose pose_des;
         std::map<std::string, geometry_msgs::msg::Pose> Poses;
 
+        // geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
+
+        // }
+
 
 }; // class MoveToPose
 

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)

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()