Change nodes for can be use wih real robot

Jepear19 · Jepear19 · commit 3839e3771d51 · 2026-07-06T19:20:12.000+02:00
diff --git a/CustomRobots/robot_arms/launch/ur3.launch.py b/CustomRobots/robot_arms/launch/ur3.launch.py
@@ -155,6 +155,7 @@ def launch_setup(context):
             {"ROB_PARAM": "ur3"},
             {"EE_PARAM": "robotiq_2f85"},
             {"ROB_GROUP": "ur3_arm"},
+            {"SYNC_REAL_ROBOT": True},
         ],
     )
 
@@ -175,6 +176,7 @@ def launch_setup(context):
             {"use_sim_time": True},
             {"ROB_PARAM": "ur3"},
             {"ROB_GROUP": "ur3_arm"},
+            {"SYNC_REAL_ROBOT": True},
         ],
     )
 
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/CMakeLists.txt b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/CMakeLists.txt
@@ -39,6 +39,8 @@ find_package(linkattacher_msgs REQUIRED)
 # REQUIRED to -> Execute ACTIONS:
 find_package(rclcpp_action REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(trajectory_msgs REQUIRED)
 
 # REQUIRED to -> Read YAML files:
 find_package(yaml-cpp REQUIRED)
@@ -57,10 +59,12 @@ add_executable(
   src/mover.cpp
   src/movel.cpp
   src/movej.cpp
+  src/trajectory_sender.cpp
   src/move.cpp
 )
 add_executable(
   robmove
+  src/trajectory_sender.cpp
   src/robmove.cpp
 )
 add_executable(
@@ -99,6 +103,8 @@ ament_target_dependencies(
   moveit_ros_planning_interface
   std_msgs
   rclcpp_action
+  control_msgs
+  trajectory_msgs
   ros2srrc_data
   yaml-cpp
   ament_index_cpp
@@ -109,6 +115,8 @@ ament_target_dependencies(
   moveit_ros_planning_interface
   std_msgs
   rclcpp_action
+  control_msgs
+  trajectory_msgs
   ros2srrc_data
 )
 ament_target_dependencies(
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/include/ros2srrc_execution/trajectory_sender.h b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/include/ros2srrc_execution/trajectory_sender.h
@@ -0,0 +1,27 @@
+#ifndef ROS2SRRC_EXECUTION_TRAJECTORY_SENDER_HPP
+#define ROS2SRRC_EXECUTION_TRAJECTORY_SENDER_HPP
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+
+class TrajectorySender
+{
+public:
+
+    explicit TrajectorySender(
+        const rclcpp::Node::SharedPtr& node
+    );
+
+    bool send(
+        const trajectory_msgs::msg::JointTrajectory& trajectory
+    );
+
+private:
+
+    rclcpp::Node::SharedPtr node_;
+
+};
+
+#endif
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/package.xml b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/package.xml
@@ -27,6 +27,9 @@
   <depend>yaml-cpp</depend>
   <depend>ament_index_cpp</depend>
 
+  <depend>control_msgs</depend>
+  <depend>trajectory_msgs</depend>
+
   <export>
     <build_type>ament_cmake</build_type>
   </export>
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/move.cpp
@@ -72,16 +72,21 @@
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
+#include "ros2srrc_execution/trajectory_sender.hpp"
+
 // Declaration of GLOBAL VARIABLES --> ROBOT / END-EFFECTOR / ENVIRONMENT PARAMETERS:
 std::string param_ROB = "none";
 std::string param_EE = "none";
 std::string param_ENV = "none";
 std::string param_ROB_GROUP = "none";
+bool sync_real_robot = false;
 
 // Declaration of GLOBAL VARIABLES --> MoveIt!2 Interface:
 moveit::planning_interface::MoveGroupInterface move_group_interface_ROB;
 moveit::planning_interface::MoveGroupInterface move_group_interface_EE;
 
+std::shared_ptr<TrajectorySender> trajectory_sender;
+
 // Declaration of GLOBAL VARIABLES --> JointModelGroup:
 const moveit::core::JointModelGroup* joint_model_group_ROB;
 const moveit::core::JointModelGroup* joint_model_group_EE;
@@ -114,6 +119,17 @@ class ros2_RobotParam : public rclcpp::Node
         RCLCPP_INFO(this->get_logger(),
             "ROB_GROUP received -> %s",
             param_ROB_GROUP.c_str());
+
+        this->declare_parameter("SYNC_REAL_ROBOT", false);
+
+        sync_real_robot =
+            this->get_parameter("SYNC_REAL_ROBOT").as_bool();
+
+        RCLCPP_INFO(
+            this->get_logger(),
+            "SYNC_REAL_ROBOT -> %s",
+            sync_real_robot ? "true" : "false"
+        );
     }
 private:
 };
@@ -432,6 +448,13 @@ class ActionServer : public rclcpp::Node
 
             rt.getRobotTrajectoryMsg(MyPlan.trajectory_);
 
+            if (sync_real_robot)
+            {
+                trajectory_sender->send(
+                    MyPlan.trajectory_.joint_trajectory
+                );
+            }
+
             bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
 
             if (goal_handle->is_canceling()) {
@@ -543,6 +566,8 @@ int main(int argc, char ** argv)
             .automatically_declare_parameters_from_overrides(true)
     );
 
+    trajectory_sender = std::make_shared<TrajectorySender>(node2);
+
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(node2);
     std::thread([&executor]() { executor.spin(); }).detach();
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/robmove.cpp
@@ -47,12 +47,17 @@
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <geometry_msgs/msg/pose.hpp>
 
+#include "ros2srrc_execution/trajectory_sender.hpp"
+
 // Declaration of GLOBAL VARIABLE --> MoveIt!2 Interface:
 moveit::planning_interface::MoveGroupInterface move_group_interface_ROB;
 
+std::shared_ptr<TrajectorySender> trajectory_sender;
+
 // Declaration of GLOBAL VARIABLE --> ROBOT PARAMETER:
 std::string param_ROB = "none";
 std::string param_ROB_GROUP = "none";
+bool sync_real_robot = false;
 
 // Declaration of GLOBAL VARIABLE --> RES:
 std::string RES = "none";
@@ -97,6 +102,17 @@ class ActionServer : public rclcpp::Node
 
         this->declare_parameter("ROB_GROUP", "none");
 
+        this->declare_parameter("SYNC_REAL_ROBOT", false);
+
+        sync_real_robot =
+            this->get_parameter("SYNC_REAL_ROBOT").as_bool();
+
+        RCLCPP_INFO(
+            this->get_logger(),
+            "SYNC_REAL_ROBOT -> %s",
+            sync_real_robot ? "true" : "false"
+        );
+
         param_ROB_GROUP =
             this->get_parameter("ROB_GROUP").as_string();
 
@@ -206,6 +222,13 @@ class ActionServer : public rclcpp::Node
 
             rt.getRobotTrajectoryMsg(MyPlan.trajectory_);
 
+            if (sync_real_robot)
+            {
+                trajectory_sender->send(
+                    MyPlan.trajectory_.joint_trajectory
+                );
+            }
+
             bool ExecSUCCESS = (move_group_interface_ROB.execute(MyPlan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
 
             if (goal_handle->is_canceling()) {
@@ -260,6 +283,8 @@ int main(int argc, char **argv)
         rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
     );
 
+    trajectory_sender = std::make_shared<TrajectorySender>(moveit_node);
+
     // Executor para que MoveIt funcione
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(moveit_node);
diff --git a/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/trajectory_sender.cpp b/Industrial/ros2_SimRealRobotControl_gz/ros2srrc_execution/src/trajectory_sender.cpp
@@ -0,0 +1,63 @@
+#include "ros2srrc_execution/trajectory_sender.hpp"
+
+TrajectorySender::TrajectorySender(
+    const rclcpp::Node::SharedPtr& node
+)
+: node_(node)
+{
+
+}
+
+bool TrajectorySender::send(
+    const trajectory_msgs::msg::JointTrajectory& trajectory
+)
+{
+    RCLCPP_INFO(
+        node_->get_logger(),
+        "========================================"
+    );
+
+    RCLCPP_INFO(
+        node_->get_logger(),
+        "Sending trajectory to real robot"
+    );
+
+    RCLCPP_INFO(
+        node_->get_logger(),
+        "Number of joints: %ld",
+        trajectory.joint_names.size()
+    );
+
+    RCLCPP_INFO(
+        node_->get_logger(),
+        "Number of points: %ld",
+        trajectory.points.size()
+    );
+
+    for (size_t i = 0; i < trajectory.points.size(); ++i)
+    {
+        const auto& p = trajectory.points[i];
+
+        std::stringstream ss;
+
+        ss << "Point " << i << " : ";
+
+        for (double q : p.positions)
+        {
+            ss << q << " ";
+        }
+
+        RCLCPP_INFO(
+            node_->get_logger(),
+            "%s",
+            ss.str().c_str()
+        );
+    }
+
+    RCLCPP_INFO(
+        node_->get_logger(),
+        "========================================"
+    );
+
+    return true;
+}

Original file line number	Diff line number	Diff line change
`@@ -155,6 +155,7 @@ def launch_setup(context):`
`155`	`155`	`{"ROB_PARAM": "ur3"},`
`156`	`156`	`{"EE_PARAM": "robotiq_2f85"},`
`157`	`157`	`{"ROB_GROUP": "ur3_arm"},`
	`158`	`+ {"SYNC_REAL_ROBOT": True},`
`158`	`159`	`],`
`159`	`160`	`)`
`160`	`161`
`@@ -175,6 +176,7 @@ def launch_setup(context):`
`175`	`176`	`{"use_sim_time": True},`
`176`	`177`	`{"ROB_PARAM": "ur3"},`
`177`	`178`	`{"ROB_GROUP": "ur3_arm"},`
	`179`	`+ {"SYNC_REAL_ROBOT": True},`
`178`	`180`	`],`
`179`	`181`	`)`
`180`	`182`