Add quaternions to forward_kinematics_1dof node

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)