updating ROS nodes

ROS Nodes/dataset_publisher.py

@@ -11,7 +11,7 @@ import random
 def read_data():
         image_paths = []
         poses = []
-        PATH = '/home/shania/catkin_ws/src/android/scripts/KingsCollege'
+        PATH = '/home/shania/catkin_ws/src/android/scripts/datasets/KingsCollege'
         file_name = 'dataset_test.txt'
         with open("/".join([PATH, file_name])) as f:
             lines = f.readlines()

ROS Nodes/draw_sequence.py

+#!/usr/bin/env python3
+
+import rospy
+import tf
+from nav_msgs.msg import Path
+from geometry_msgs.msg import PoseStamped
+
+
+def read_data():
+	image_paths = []
+	poses = []
+	PATH = '/home/shania/catkin_ws/src/android/scripts/datasets/TurtlebotTourCVSSP/'
+	file_name = 'metadata.txt'
+	with open("/".join([PATH, file_name])) as f:
+		lines = f.readlines()
+	for i in range(3, len(lines)):  # skipping first 3 lines
+		data = lines[i].split()
+		image_paths.append("/".join([PATH, data[0]]))
+		poses.append([float(x) for x in data[1:]])
+    
+	return image_paths, poses
+        
+        
+def create_path_message():
+	# Initialize a new Path message
+	path_msg = Path()    	
+	
+		# Extract poses and add them to the path
+	_, poses = read_data()
+	for i in range(len(poses)):
+		# print(poses[i])
+		pose_msg = PoseStamped()
+		pose_msg.header.stamp = rospy.Time.now()  # Set the timestamp
+		pose_msg.header.frame_id = "map-gt" 
+		pose_msg.pose.position.x = poses[i][0]  # X coordinate
+		pose_msg.pose.position.y = poses[i][1]  # Y coordinate
+		pose_msg.pose.position.z = poses[i][2]  # Z coordinate
+		# You can set orientation if needed
+		pose_msg.pose.orientation.x = poses[i][3]
+		pose_msg.pose.orientation.y = poses[i][4]
+		pose_msg.pose.orientation.z = poses[i][5]
+		pose_msg.pose.orientation.w = poses[i][6]
+        
+        # Add the pose to the path
+		# print(pose_msg)
+		path_msg.poses.append(pose_msg)
+	
+	path_msg.header.stamp = rospy.Time.now()  # Set the timestamp
+	path_msg.header.frame_id = "map-gt" 
+	
+	# Apply transform to path message	
+	# t_msg = listener.transformPose("map", path_msg)  
+	# t_msg.header.stamp = rospy.Time.now()
+    
+	return path_msg
+
+
+if __name__ == '__main__':
+	rospy.init_node('path_publisher')
+    
+	# Create a publisher for the path message
+	path_publisher = rospy.Publisher('/path_topic', Path, queue_size=10)
+    
+	rate = rospy.Rate(1)  # 1 Hz
+    
+	while not rospy.is_shutdown():
+		# Create and publish the path message
+		# listener = tf.TransformListener()  # Don't need this as a transform is running, all we need is to state the original frame when publishing
+		path_msg = create_path_message()
+		path_publisher.publish(path_msg)
+		rospy.loginfo("Path message published.")
+		rate.sleep()
+

ROS Nodes/models.py

+import torch
+import torch.nn as nn
+import torch.nn.functional as F
+from torchvision import models
+
+
+# https://github.com/NVlabs/geomapnet/blob/master/models/posenet.py
+class PoseNet(nn.Module):
+    def __init__(self, droprate=0.5, pretrained=True):
+        super(PoseNet, self).__init__()
+        self.droprate = droprate
+
+        # replace the last FC layer in feature extractor
+        self.feature_extractor = models.resnet34(weights="IMAGENET1K_V1")
+        self.feature_extractor.avgpool = nn.AdaptiveAvgPool2d(1)
+        in_dim = self.feature_extractor.fc.in_features
+        self.feature_extractor.fc = nn.Linear(in_dim, 2048)
+
+        self.fc_xyz = nn.Linear(2048, 3)
+        self.fc_wpqr = nn.Linear(2048, 4)
+
+        # initialize
+        if pretrained:
+            init_modules = [self.feature_extractor.fc, self.fc_xyz, self.fc_wpqr]
+        else:
+            init_modules = self.modules()
+
+        for m in init_modules:
+            if isinstance(m, nn.Conv2d) or isinstance(m, nn.Linear):
+                nn.init.kaiming_normal_(m.weight.data)
+                if m.bias is not None:
+                    nn.init.constant_(m.bias.data, 0)
+
+    def forward(self, x):
+        x = self.feature_extractor(x)
+        # 1 x 2048
+        x = F.relu(x)
+        if self.droprate > 0:
+            x = F.dropout(x, p=self.droprate)
+
+        xyz = self.fc_xyz(x)
+        wpqr = self.fc_wpqr(x)
+        return xyz, wpqr

ROS Nodes/photo_subscriber.py

-#!/usr/bin/env python3
-
-import rospy
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge
-import cv2
-
-def callback(data):
-	print("Received data of type: ", type(data))
-	br = CvBridge()
-	data = br.imgmsg_to_cv2(data, "bgr8")
-	print("Converted to type: ", type(data))
-	cv2.imshow("Smartphone Image", data)
-	
-	cv2.waitKey(0)
-	 
-
-if __name__ == '__main__':
-	rospy.init_node("photo_subscriber")
-	sub = rospy.Subscriber("/photo", Image, callback)
-	rospy.loginfo("Photo Node started")
-	
-	while not rospy.is_shutdown():
-		rospy.spin()
-	cv2.destroyAllWindows()
-	

ROS Nodes/pose_publisher.py

@@ -8,6 +8,7 @@ from geometry_msgs.msg import PoseStamped
 from cv_bridge import CvBridge
 from PIL import Image as Img
 import cv2
+import tf
 
 import torch
 from torchvision import transforms
@@ -15,6 +16,7 @@ import torch.nn.functional as F
 
 import models
 
+listener = None
 
 def image_callback(data):
 	# ROS Format to cv2 
@@ -40,8 +42,8 @@ def image_callback(data):
 	print("INPUT: ", photo.shape)
 	
 	# MODEL
-	model = models.ResNet()
-	PATH = '/home/shania/catkin_ws/src/android/scripts/model.pth'
+	model = models.PoseNet()
+	PATH = '/home/shania/catkin_ws/src/android/scripts/posenet_300.pth'
 	model.load_state_dict(torch.load(PATH))
 	model.eval()
 		
@@ -52,23 +54,33 @@ def image_callback(data):
 	print(pos_out, ori_out)	
 	
 	msg = PoseStamped()  # geometry pose stamped
-	msg.header.stamp = rospy.Time.now()
-	msg.header.frame_id = "pose"
-	
+	msg.header.stamp = rospy.Time(0)
+	msg.header.frame_id = "map-gt"  # this is gt coordinate frame; we then use a node to transform it from map-gt to map
 	msg.pose.position.x = pos_out[0]
 	msg.pose.position.y = pos_out[1]
-	msg.pose.position.z = pos_out[2]
-	msg.pose.orientation.x = ori_out[0]
-	msg.pose.orientation.y = ori_out[1]
-	msg.pose.orientation.z = ori_out[2]
-	msg.pose.orientation.w = ori_out[3]	
+	msg.pose.position.z = 0.0
+	msg.pose.orientation.x = 0
+	msg.pose.orientation.y = 0
+	msg.pose.orientation.z = 0
+	msg.pose.orientation.w = 1
+	
+	# t_msg = listener.transformPose("butthead/map", msg)  # listen to any transforms being broadcast in ROS and convert to this frame; the pubished pose is in the frame butthead/map
+	# NOT CORRECT
+	# Don't need this as a transform is running, all we need is to state the original frame when publishing, rosrun tf handles the rest
+	# t_msg.header.stamp = rospy.Time.now()
+	# t_msg.pose.position.z = 0
+	# t_msg.pose.orientation.x = 0
+	# t_msg.pose.orientation.y = 0
+	# t_msg.pose.orientation.z = 0
+	# t_msg.pose.orientation.w = 1
+	
 	pub.publish(msg)  
 	
 
 if __name__ == '__main__':
 	rospy.init_node("pose_publisher")
-	
-	pub = rospy.Publisher("/camera_pose", PoseStamped, queue_size=10)
+	# listener = tf.TransformListener()
+	pub = rospy.Publisher("/butthead/move_base_simple/goal", PoseStamped, queue_size=10)
 	sub = rospy.Subscriber("/photo", Image, callback=image_callback)
 	rospy.loginfo("Node has been started")