From 806cb7213076ed451c19a5261e9603cbabd7895d Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Thu, 27 Jun 2024 13:16:55 +0200
Subject: [PATCH] Add quaternions to forward_kinematics_1dof node
---
.../forward_kinematics_node_1dof.py | 17 ++++++++++-------
1 file changed, 10 insertions(+), 7 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 15e210e..19da4a3 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
@@ -6,6 +6,7 @@ from std_msgs.msg import Float64
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker
+import transformations
class Forward_Kinematics_Node_1Dof(Node):
@@ -23,7 +24,7 @@ class Forward_Kinematics_Node_1Dof(Node):
# 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)
+ self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose_1dof', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -56,8 +57,10 @@ class Forward_Kinematics_Node_1Dof(Node):
# Calculate the intermediate vectors
intermediate_vector = np.matmul(intermediate_rotation, start_vector)
+
+ quaternion = transformations.quaternion_from_euler(0.0, 0.0, theta_fraction)
- self.publish_pose(intermediate_vector)
+ self.publish_pose(intermediate_vector, quaternion)
self.construct_path(intermediate_vector)
@@ -109,17 +112,17 @@ class Forward_Kinematics_Node_1Dof(Node):
self.angle_publisher_.publish(angle_msg)
- def publish_pose(self, vector):
+ def publish_pose(self, vector, quaternion):
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
+ pose_msg.pose.orientation.x = quaternion[0]
+ pose_msg.pose.orientation.y = quaternion[1]
+ pose_msg.pose.orientation.z = quaternion[2]
+ pose_msg.pose.orientation.w = quaternion[3]
self.pose_publisher_.publish(pose_msg)
--
GitLab