diff --git a/src/Bringup/launch/antenna.launch.py b/src/Bringup/launch/antenna.launch.py index cbbe7b36..ca7811cb 100644 --- a/src/Bringup/launch/antenna.launch.py +++ b/src/Bringup/launch/antenna.launch.py @@ -29,8 +29,8 @@ def generate_launch_description(): }, {"Baudrate": 115200}, {"Freq": 5.0}, # Publish rate (hz) - {"SvinMindur": 300}, # Survey in time (s) - {"SvinMinAccDur": 10_000}, # Survey in accuracy (mm) + {"SvinMinDur": 300}, # Survey-in minimum duration (s) + {"SvinAccLimit": 10_000}, # Survey-in accuracy limit (mm) {"QueueDepth": 10}, ], ), @@ -61,7 +61,10 @@ def generate_launch_description(): "CountsPerRev": 8192 # based on 4096 encoder resolution and 2:1 gear ratio }, ], - remappings=[("/roboclaw_position", "/antenna/tracker_bearing")], + remappings=[ + ("/roboclaw_desired_position", "/antenna/target_bearing"), + ("/roboclaw_actual_position", "/antenna/bearing"), + ], ), # Optionally run the standalone basestation config script when requested ExecuteProcess( diff --git a/src/HW-Devices/ros_roboclaw/ros_roboclaw/antenna_roboclaw_node.py b/src/HW-Devices/ros_roboclaw/ros_roboclaw/antenna_roboclaw_node.py index 2488e994..401e0511 100644 --- a/src/HW-Devices/ros_roboclaw/ros_roboclaw/antenna_roboclaw_node.py +++ b/src/HW-Devices/ros_roboclaw/ros_roboclaw/antenna_roboclaw_node.py @@ -50,18 +50,23 @@ def __init__(self): self.zero_offset = 0 # ROS Interfaces - self.create_subscription(Float32, "/roboclaw_position", self.pos_callback, 5) + self.create_subscription( + Float32, "/roboclaw_desired_position", self.pos_callback, 5 + ) self.create_timer(1.0 / self.enc_read_freq, self.encoder_timer) + self.antenna_encoder_pub = self.create_publisher( + Float32, "/roboclaw_actual_position", 10 + ) # Angle to Encoder Command def pos_callback(self, msg: Float32): - norm = msg.data + norm = (float(msg.data) % (math.pi * 2)) / (math.pi * 2) self.target_encoder = self.zero_offset + int(norm * self.counts_per_rev) error = self.wrap_error(self.target_encoder - self.current_encoder) - self.get_logger().info(f"target= {norm * 360:.2f}°, {self.target_encoder}") + # self.get_logger().info(f"target= {norm * 360:.2f}°, {self.target_encoder}") self.drive_to_position(self.current_encoder + error) @@ -82,6 +87,10 @@ def encoder_timer(self): return self.current_encoder = enc + enc_rel = (enc - self.zero_offset) % self.counts_per_rev + antenna_radians = Float32() + antenna_radians.data = (float(enc_rel) / self.counts_per_rev) * (math.pi * 2) + self.antenna_encoder_pub.publish(antenna_radians) # Position Control def drive_to_position(self, target): diff --git a/src/Nav/gps/gps/antenna_pointing_node.py b/src/Nav/gps/gps/antenna_pointing_node.py index b5b8d90e..e5733121 100644 --- a/src/Nav/gps/gps/antenna_pointing_node.py +++ b/src/Nav/gps/gps/antenna_pointing_node.py @@ -33,9 +33,7 @@ def __init__(self): self.create_subscription(NavSatFix, "/base_station/fix", self.base_cb, 2) self.create_subscription(NavSatFix, "/gps/fix", self.rover_cb, 2) - self.bearing_pub = self.create_publisher( - Float32, "/antenna/tracker_bearing", 10 - ) + self.bearing_pub = self.create_publisher(Float32, "/antenna/target_bearing", 10) self.timer = self.create_timer(1.0 / self.freq, self.update) @@ -76,9 +74,7 @@ def update(self): def publish_bearing(self, bearing): msg = Float32() - msg.data = float(bearing) / ( - math.pi * 2 - ) # normalize the output for the roboclaw + msg.data = float(bearing) # radians follow the ros standard, don't normalize self.bearing_pub.publish(msg) def bearing(self, lat1, lon1, lat2, lon2):