From e3c886bdccefd7db778996b34715403f2a19d70b Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Wed, 26 Jun 2024 17:28:23 +0200
Subject: [PATCH] Create a publisher for the pose in '2dof' node
---
.../forward_kinematics_node_2dof_1.py | 17 +++++++++++++++++
1 file changed, 17 insertions(+)
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
index 07d8cac..58a8719 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
@@ -23,6 +23,8 @@ class Forward_Kinematics_Node_2Dof(Node):
# create a publisher for the plotjuggler data
self.angle_publisher_ = self.create_publisher(Float64, '/angle_2dof/plotjuggler', 10)
self.height_publisher_ = self.create_publisher(Float64, '/height_2dof/plotjuggler', 10)
+ # create a publisher for the pose
+ self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose_2dof', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -63,6 +65,8 @@ class Forward_Kinematics_Node_2Dof(Node):
# Calculate the intermediate vectors
intermediate_vector = np.matmul(intermediate_rotation, start_vector)
+ self.publish_pose(intermediate_vector)
+
self.construct_path(intermediate_vector)
def construct_path(self, vector):
@@ -117,6 +121,19 @@ class Forward_Kinematics_Node_2Dof(Node):
height_msg.data = height
self.height_publisher_.publish(height_msg)
+ def publish_pose(self, vector):
+ pose_msg = PoseStamped()
+ pose_msg.header.frame_id = "map"
+ pose_msg.header.stamp = self.get_clock().now().to_msg()
+ pose_msg.pose.position.x = vector[0]
+ pose_msg.pose.position.y = vector[1]
+ pose_msg.pose.position.z = vector[2]
+ pose_msg.pose.orientation.x = 0.0
+ pose_msg.pose.orientation.y = 0.0
+ pose_msg.pose.orientation.z = 0.0
+ pose_msg.pose.orientation.w = 1.0
+ self.pose_publisher_.publish(pose_msg)
+
def main(args = None):
rclpy.init(args = args)
--
GitLab