Notes

"""
my_node.py

A ROS node that repeats the map and odometry topic to the correct ecte477
namespace topics for map and path.

Subscribed: map/, odom/
Publishes: ecte477/map/, ecte477/e_path/, ecte477/r_path/
Services: explore/explore_service
Created for nadir_demo_1
"""
import rclpy
from rclpy.node import Node
import time
from nav_msgs.msg import OccupancyGrid, Odometry, Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import MarkerArray
from std_msgs.msg import String

from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy

class my_node(Node):
def __init__(self):
# Initialise subs, pubs, service calls, path object
super().__init__('nadir_demo_node')

# Define QoS profile
qos_policy = QoSProfile(
durability=QoSDurabilityPolicy.TRANSIENT_LOCAL,
reliability=QoSReliabilityPolicy.RELIABLE,
history=QoSHistoryPolicy.KEEP_LAST,
depth=1
)

# Initialize state flags
self.exploring = False
self.exploration_complete = False
self.returning = False

# Initialize paths for requirement 6
self.exploration_path = Path()
self.exploration_path.header.frame_id = 'odom'

self.return_path = Path()
self.return_path.header.frame_id = 'odom'

##### Publishers ######
# Requirement 3: Republish map to correct topic
self.map_pub = self.create_publisher(
OccupancyGrid,
'/ecte477/maze',
qos_policy
)

# Requirement 2: Start exploration command
self.start_pub = self.create_publisher(
String,
'/start_explore',
10
)

# Requirement 5: Publishing goal for returning home
self.goal_pub = self.create_publisher(
PoseStamped,
'/goal_pose',
10
)

# Requirement 6: Publishing paths
self.e_path_pub = self.create_publisher(
Path,
'/ecte477/e_path',
10
)

self.r_path_pub = self.create_publisher(
Path,
'/ecte477/r_path',
10
)

##### Subscribers #####
# Requirement 3: Subscribe to map
self.map_sub = self.create_subscription(
OccupancyGrid,
'/map',
self.callback_map,
qos_policy
)

# Requirement 6: Subscribe to odometry
self.odom_sub = self.create_subscription(
Odometry,
'/odom',
self.callback_odom,
10
)

# Requirement 4: Subscribe to stack_points
self.stack_sub = self.create_subscription(
MarkerArray,
'/stack_points',
self.callback_stack_points,
10
)

# Timer to check if maze_explorer3 is ready before sending the command
self.ready_check_timer = self.create_timer(1.0, self.check_ready)
self.start_timer = None

self.get_logger().info('Nadir Demo 1 node initialized. Waiting until maze_explorer3 is ready...')

def check_ready(self):
# Check if publisher has subscribers before starting the exploration timer
if self.start_pub.get_subscription_count() > 0:
self.get_logger().info('Maze explorer node is ready! Starting 8-second countdown...')
self.start_timer = self.create_timer(8.0, self.start_exploration)
self.ready_check_timer.cancel() # Stop checking

def callback_map(self, data):
# Requirement 3: Republish map to /ecte477/maze
self.map_pub.publish(data)

def callback_odom(self, data):
# Requirement 6: Track and publish robot paths
current_pose = PoseStamped()
current_pose.header = data.header
current_pose.pose = data.pose.pose

# Adding the pose to the appropriate path based on current mode
if self.exploring:
# Add to exploration path
self.exploration_path.poses.append(current_pose)
self.exploration_path.header.stamp = self.get_clock().now().to_msg()
self.e_path_pub.publish(self.exploration_path)

elif self.returning:
# Add to return path
self.return_path.poses.append(current_pose)
self.return_path.header.stamp = self.get_clock().now().to_msg()
self.r_path_pub.publish(self.return_path)

def callback_stack_points(self, stack_points):
# Requirement 4: Detect when exploration is complete
if self.exploring and not self.exploration_complete:
# Check if markers array size is 0, which indicates exploration is complete
if len(stack_points.markers) == 0:
self.get_logger().info('Exploration complete, waiting 5 seconds before returning home...')
self.exploring = False
self.exploration_complete = True

# Wait 5 seconds before returning home
self.create_timer(5.0, self.return_home)

def start_exploration(self):
# Requirement 2: Send command to start exploration
msg = String()
msg.data = "START"
self.start_pub.publish(msg)
self.exploring = True
self.get_logger().info('Starting exploration...')
if self.start_timer:
self.start_timer.cancel() # Cancel timer

def return_home(self):
# Requirement 5: Send robot back to origin (0, 0, 0)
goal_msg = PoseStamped()
goal_msg.header.stamp = self.get_clock().now().to_msg()
goal_msg.header.frame_id = 'map'

# Set target position to origin (0, 0, 0)
goal_msg.pose.position.x = 0.0
goal_msg.pose.position.y = 0.0
goal_msg.pose.position.z = 0.0

# Set orientation (w=1.0 is set)
goal_msg.pose.orientation.x = 0.0
goal_msg.pose.orientation.y = 0.0
goal_msg.pose.orientation.z = 0.0
goal_msg.pose.orientation.w = 1.0

self.goal_pub.publish(goal_msg)
self.returning = True
self.get_logger().info('Sending robot back to origin (0, 0, 0)...')

# Main function
def main(args=None):
rclpy.init(args=args)

mn = my_node()

rclpy.spin(mn)

mn.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':
main()