From 4df2110077d1772800037b7486afe13e8b6603cc Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Wed, 26 Jun 2024 16:15:06 +0200
Subject: [PATCH] Create a publisher for the pose of the end effector
---
.../forward_kinematics_node_1dof.py | 25 ++++++++++++++++---
1 file changed, 22 insertions(+), 3 deletions(-)
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
index c36fabe..15e210e 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
@@ -22,6 +22,8 @@ class Forward_Kinematics_Node_1Dof(Node):
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
# create a publisher for the plotjuggler angles
self.angle_publisher_ = self.create_publisher(Float64, '/angles_1dof/plotjuggler', 10)
+ # create a publisher for the pose
+ self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -35,14 +37,14 @@ class Forward_Kinematics_Node_1Dof(Node):
def transformation(self):
# Define the desired rotation here
- theta = - pi / 2
+ theta = pi / 2
# Split theta
theta_fraction = self.phi * theta
self.publish_angle(theta_fraction)
# Define the vector
- start_vector = np.array([-1.0, 0.0, 0.0, 1.0])
+ start_vector = np.array([1.0, 0.0, 0.0, 1.0])
# Define the matrix
intermediate_rotation = np.array([
@@ -54,7 +56,9 @@ class Forward_Kinematics_Node_1Dof(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):
@@ -104,6 +108,21 @@ class Forward_Kinematics_Node_1Dof(Node):
angle_msg.data = angle * 180 / pi
self.angle_publisher_.publish(angle_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)
node = Forward_Kinematics_Node_1Dof()
--
GitLab