Change plugin ros2_control classic to gazebo

Author: Jepear19

Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro

@@ -79,13 +79,10 @@
   <xacro:unless value="${bringup}">
 
     <gazebo>
-      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-        <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
-
-        <!-- UR5 Controller -->
+      <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control">
+        
         <parameters>/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/config/controller.yaml</parameters>
 
-        <!-- End-Effector Controller (if needed) -->
         <parameters>/home/ws/src/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_endeffectors/${EE_name}/config/controller.yaml</parameters>
 
       </plugin>

Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp

@@ -172,7 +172,7 @@ class ActionServer : public rclcpp::Node
 
         action_server_ = rclcpp_action::create_server<Move>(
             this,
-            "/Move",
+            "/move",
             std::bind(&ActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
             std::bind(&ActionServer::handle_cancel, this, std::placeholders::_1),
             std::bind(&ActionServer::handle_accepted, this, std::placeholders::_1));
@@ -430,73 +430,107 @@ class ActionServer : public rclcpp::Node
 
 int main(int argc, char ** argv)
 {
-    // Initialise MAIN NODE:
+    // ================= INIT =================
     rclcpp::init(argc, argv);
     auto const logger = rclcpp::get_logger("MOVE_INTERFACE");
 
-    // Obtain ROBOT + END-EFFECTOR + ENVIRONMENT parameters:
+    // ================= PARAMETERS =================
     auto node_PARAM_ROB = std::make_shared<ros2_RobotParam>();
     rclcpp::spin_some(node_PARAM_ROB);
+
     auto node_PARAM_EE = std::make_shared<ros2_EEParam>();
     rclcpp::spin_some(node_PARAM_EE);
 
-    // DEFINE -> RobotSPECS + eeSPECS variables:
-    // Robot SPECIFICATIONS:
+    // ================= LOAD SPECS =================
     if (param_ROB != "none"){
-        std::string pkgPATH_R = ament_index_cpp::get_package_share_directory("ros2srrc_robots");
-        std::string PATH_R = pkgPATH_R + "/" + param_ROB + "/config/joint_specifications.yaml";
+        std::string pkgPATH_R =
+            ament_index_cpp::get_package_share_directory("ros2srrc_robots");
+        std::string PATH_R =
+            pkgPATH_R + "/" + param_ROB + "/config/joint_specifications.yaml";
+
         YAML::Node SPECIFICATIONS_R = YAML::LoadFile(PATH_R);
-        robotSPECS.robot_max = SPECIFICATIONS_R["Limits"]["Max"].as<std::vector<double>>();
-        robotSPECS.robot_min = SPECIFICATIONS_R["Limits"]["Min"].as<std::vector<double>>();
-    };
-    // End-Effector SPECIFICATIONS:
+
+        robotSPECS.robot_max =
+            SPECIFICATIONS_R["Limits"]["Max"].as<std::vector<double>>();
+        robotSPECS.robot_min =
+            SPECIFICATIONS_R["Limits"]["Min"].as<std::vector<double>>();
+    }
+
     if (param_EE != "none"){
-        std::string pkgPATH = ament_index_cpp::get_package_share_directory("ros2srrc_endeffectors");
-        std::string PATH = pkgPATH + "/" + param_EE + "/config/joint_specifications.yaml";
+        std::string pkgPATH =
+            ament_index_cpp::get_package_share_directory("ros2srrc_endeffectors");
+        std::string PATH =
+            pkgPATH + "/" + param_EE + "/config/joint_specifications.yaml";
+
         YAML::Node SPECIFICATIONS = YAML::LoadFile(PATH);
+
         eeSPECS.ee_max = SPECIFICATIONS["Limits"]["Max"].as<double>();
         eeSPECS.ee_min = SPECIFICATIONS["Limits"]["Min"].as<double>();
-        eeSPECS.ee_vector =  SPECIFICATIONS["JointsVector"].as<std::vector<double>>();
-    };
+        eeSPECS.ee_vector =
+            SPECIFICATIONS["JointsVector"].as<std::vector<double>>();
+    }
 
-    // Launch and spin (EXECUTOR) MoveIt!2 Interface node:
-    auto name = "ros2srrc_move";
-    auto const node2 = std::make_shared<rclcpp::Node>(name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
-    rclcpp::executors::SingleThreadedExecutor executor; 
+    // ================= ACTION SERVER (CLAVE) =================
+    auto action_server = std::make_shared<ActionServer>();
+
+    auto node2 = std::make_shared<rclcpp::Node>(
+        "moveit_client_node",
+        rclcpp::NodeOptions()
+            .automatically_declare_parameters_from_overrides(true)
+    );
+    
+    rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(node2);
     std::thread([&executor]() { executor.spin(); }).detach();
 
-    // CREATE -> MoveGroupInterface(s):
+    // Esperar a que MoveIt publique parámetros
+    rclcpp::sleep_for(std::chrono::seconds(2));
+
+
+    // ================= MOVEIT =================
     using moveit::planning_interface::MoveGroupInterface;
-    // 1. ROBOT:
+
+    // ROBOT
     if (param_ROB != "none"){
-        auto name = param_ROB + "_arm";
-        
-        move_group_interface_ROB = MoveGroupInterface(node2, name);
+        std::string group_name = "manipulator";
+
+        move_group_interface_ROB = MoveGroupInterface(node2, group_name);
         move_group_interface_ROB.setPlanningPipelineId("move_group");
 
         move_group_interface_ROB.setMaxVelocityScalingFactor(1.0);
         move_group_interface_ROB.setMaxAccelerationScalingFactor(1.0);
 
-        joint_model_group_ROB = move_group_interface_ROB.getCurrentState()->getJointModelGroup(name);
-        RCLCPP_INFO(logger, "MoveGroupInterface object created for ROBOT: %s", param_ROB.c_str());
+        joint_model_group_ROB =
+            move_group_interface_ROB.getCurrentState()
+                ->getJointModelGroup(group_name);
+
+        RCLCPP_INFO(logger,
+            "MoveGroupInterface created for ROBOT group: %s",
+            group_name.c_str());
     }
-    // 2. END-EFFECTOR:
+
+    // END EFFECTOR
     if (param_EE != "none"){
         move_group_interface_EE = MoveGroupInterface(node2, param_EE);
         move_group_interface_EE.setPlanningPipelineId("move_group");
+
         move_group_interface_EE.setMaxVelocityScalingFactor(1.0);
         move_group_interface_EE.setMaxAccelerationScalingFactor(1.0);
-        joint_model_group_EE = move_group_interface_EE.getCurrentState()->getJointModelGroup(param_EE);
-        RCLCPP_INFO(logger, "MoveGroupInterface object created for END-EFFECTOR: %s", param_EE.c_str());
+
+        joint_model_group_EE =
+            move_group_interface_EE.getCurrentState()
+                ->getJointModelGroup(param_EE);
+
+        RCLCPP_INFO(logger,
+            "MoveGroupInterface created for EE: %s",
+            param_EE.c_str());
     }
 
-    // CREATE -> PlanningSceneInterface:
+    // ================= PLANNING SCENE =================
     using moveit::planning_interface::PlanningSceneInterface;
     auto planning_scene_interface = PlanningSceneInterface();
 
-    // Declare and spin ACTION SERVER:
-    auto action_server = std::make_shared<ActionServer>();
+    // ================= SPIN =================
     rclcpp::spin(action_server);
 
     rclcpp::shutdown();

Industrial/ros2_SimRealRobotControl_gz/ros2srrc_robots/ur5/urdf/ur5_ros2control.xacro

@@ -144,7 +144,7 @@
       <!-- A. GAZEBO SIMULATION: -->
       <xacro:unless value="${bringup}">
         <hardware>
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+          <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </hardware>
       </xacro:unless>
 

Launchers/pick_place_harmonic/pick_place_harmonic.launch.py

@@ -37,7 +37,7 @@ def generate_launch_description():
 
     gz = ExecuteProcess(
         cmd=["gz", "sim", "-r", "-v", "4", world_path],
-        output="screen"
+        output="both"
     )
 
     # =========================
@@ -89,16 +89,21 @@ def generate_launch_description():
     robot_state_publisher = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
-        output="screen",
+        output="both",
+        arguments=["--ros-args", "--log-level", "debug"],
         parameters=[robot_description, {"use_sim_time": True}],
     )
 
-    static_tf = Node(
+    """static_tf = Node(
         package="tf2_ros",
         executable="static_transform_publisher",
-        arguments=["0", "0", "0.9", "0", "0", "0", "world", "base_link"],
+        arguments=[
+            "0", "0", "0.9", "0", "0", "0", "world", "base_link",
+            "--ros-args", "--log-level", "debug",
+        ],
+        output="both",
         parameters=[{"use_sim_time": True}],
-    )
+    )"""
 
     # =========================
     # SPAWN ROBOT (HARMONIC)
@@ -108,13 +113,14 @@ def generate_launch_description():
         package="ros_gz_sim",
         executable="create",
         arguments=[
-            "-topic", "robot_description",
+            "-param", "robot_description",
             "-name", "ur5",
             "-x", "0.0",
             "-y", "0.0",
             "-z", "0.9",
         ],
-        output="screen",
+        parameters=[robot_description],
+        output="both",
     )
 
     # =========================
@@ -153,15 +159,32 @@ def generate_launch_description():
     # =========================
     # MOVE GROUP (IGUAL QUE CLASSIC)
     # =========================
+    
+
+    moveit_controllers = {
+        "moveit_simple_controller_manager": load_yaml(
+            "ur5_gripper_moveit_config",
+            "config/moveit_controllers.yaml"
+        )
+    }
+
+    print("===== DEBUG MOVEIT =====")
+    print(robot_description.keys())
+    print(robot_description_semantic.keys())
+    print(moveit_controllers)
+    print("========================")
 
     move_group = Node(
         package="moveit_ros_move_group",
         executable="move_group",
-        output="screen",
+        output="both",
+        arguments=["--ros-args", "--log-level", "debug"],
         parameters=[
             robot_description,
             robot_description_semantic,
             kinematics_yaml,
+            moveit_controllers,
+            {"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager"},
             {"use_sim_time": True},
         ],
     )
@@ -183,21 +206,24 @@ def generate_launch_description():
     move_node = Node(
         package="ros2srrc_execution",
         executable="move",
-        output="screen",
+        output="both",
+        arguments=["--ros-args", "--log-level", "debug"],
         parameters=common_params,
     )
 
     robmove_node = Node(
         package="ros2srrc_execution",
         executable="robmove",
-        output="screen",
+        output="both",
+        arguments=["--ros-args", "--log-level", "debug"],
         parameters=common_params,
     )
 
     robpose_node = Node(
         package="ros2srrc_execution",
         executable="robpose",
-        output="screen",
+        output="both",
+        arguments=["--ros-args", "--log-level", "debug"],
         parameters=common_params,
     )
 
@@ -209,29 +235,44 @@ def generate_launch_description():
 
         gz,
         robot_state_publisher,
-        static_tf,
+        # static_tf,
         clock_bridge,
 
         spawn_robot,
 
         RegisterEventHandler(
             OnProcessExit(
                 target_action=spawn_robot,
-                on_exit=[joint_state_broadcaster],
+                on_exit=[
+                    TimerAction(
+                        period=3.0,
+                        actions=[joint_state_broadcaster],
+                    )
+                ],
             )
         ),
 
         RegisterEventHandler(
             OnProcessExit(
                 target_action=joint_state_broadcaster,
-                on_exit=[joint_trajectory_controller],
+                on_exit=[
+                    TimerAction(
+                        period=2.0,
+                        actions=[joint_trajectory_controller],
+                    )
+                ],
             )
         ),
 
         RegisterEventHandler(
             OnProcessExit(
                 target_action=joint_trajectory_controller,
-                on_exit=[gripper_controller],
+                on_exit=[
+                    TimerAction(
+                        period=2.0,
+                        actions=[gripper_controller],
+                    )
+                ],
             )
         ),
 
@@ -240,7 +281,7 @@ def generate_launch_description():
                 target_action=spawn_robot,
                 on_exit=[
                     TimerAction(
-                        period=2.0,
+                        period=5.0,
                         actions=[move_group],
                     )
                 ],
@@ -249,10 +290,10 @@ def generate_launch_description():
 
         RegisterEventHandler(
             OnProcessExit(
-                target_action=spawn_robot,
+                target_action=move_group,
                 on_exit=[
                     TimerAction(
-                        period=4.0,
+                        period=5.0,
                         actions=[move_node, robmove_node, robpose_node],
                     )
                 ],