Qol fixes aug2

setup/start_rover.sh

@@ -11,7 +11,7 @@ if [ -z "$web_server_id" ]; then
         --ipc host \
         --name web-server \
         $ROVER_DOCKER_IMAGE \
-        ros2 launch rosbridge_server rosbridge_websocket_launch.xml
+        ros2 launch bringup websocket.launch.py
 else
     echo "Web server is already running."
 fi

src/Bringup/launch/websocket.launch.py

@@ -0,0 +1,21 @@
+import os
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    rosbridge_websocket = Node(
+        package="rosbridge_server",
+        executable="rosbridge_websocket",
+        name="rosbridge_websocket",
+        parameters=[
+            {"default_call_service_timeout": 60.0},
+            {"max_message_size": 10000000},
+        ],
+    )
+
+    ld = LaunchDescription()
+    ld.add_action(rosbridge_websocket)
+    return ld

src/Cameras/video_streaming/config/presets.yaml

@@ -8,6 +8,7 @@ preset_node:
     - "DriveEEFPreset"
     - "EEFDrivePreset"
     - "EEFDriveMast"
+    - "DriveMastPreset"
     DrivePreset:
       name: "Drive"
       sources:
@@ -94,4 +95,19 @@ preset_node:
         width: 40
         height: 40
         origin_x: 0
+        origin_y: 0
+    DriveMastPreset:
+      name: "Drive + Mast"
+      sources:
+      - "Drive"
+      - "Mast"
+      Drive:
+        width: 100
+        height: 100
+        origin_x: 0
+        origin_y: 0
+      Mast:
+        width: 30
+        height: 30
+        origin_x: 70
         origin_y: 0

src/Cameras/video_streaming/include/video_streaming/preset_node.hpp

@@ -4,7 +4,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include "interfaces/msg/video_preset.hpp"
-#include "interfaces/srv/get_presets.hpp"
+#include "interfaces/msg/video_presets.hpp"
 
 class PresetNode : public rclcpp::Node {
 public:
@@ -16,7 +16,7 @@ class PresetNode : public rclcpp::Node {
   void load_presets();
 
   std::vector<interfaces::msg::VideoPreset> presets_;
-  rclcpp::Service<interfaces::srv::GetPresets>::SharedPtr list_presets_service_;
+  rclcpp::Publisher<interfaces::msg::VideoPresets>::SharedPtr presets_pub_;
 };
 
 #endif // PRESET_NODE_HPP

src/Cameras/video_streaming/src/preset_node.cpp

@@ -6,13 +6,15 @@ PresetNode::PresetNode(const rclcpp::NodeOptions &options)
   declare_parameters();
   load_presets();
 
-  list_presets_service_ = this->create_service<interfaces::srv::GetPresets>(
-      "/list_presets",
-      [this](
-          const std::shared_ptr<interfaces::srv::GetPresets::Request> request,
-          std::shared_ptr<interfaces::srv::GetPresets::Response> response) {
-        response->presets = presets_;
-      });
+  auto presets_qos =
+      rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
+
+  presets_pub_ = this->create_publisher<interfaces::msg::VideoPresets>(
+      "/video_presets", presets_qos);
+
+  interfaces::msg::VideoPresets presets_msg;
+  presets_msg.presets = presets_;
+  presets_pub_->publish(presets_msg);
 
   RCLCPP_INFO(this->get_logger(), "PresetNode started");
 }

src/HW-Devices/science_sensors/science_sensors/panoramic.py

@@ -48,6 +48,15 @@ def move_servo(publisher, angle):
         msg.data = angle
         publisher.publish(msg)
 
+    @staticmethod
+    def crop_bottom(
+        image: npt.NDArray[np.uint8], percent: float = 0.10
+    ) -> npt.NDArray[np.uint8]:
+        percent = max(0.0, min(float(percent), 1.0))
+        h = image.shape[0]
+        new_h = int(h * (1.0 - percent))
+        return image[:new_h, :]
+
     def capture_image(self) -> npt.NDArray[np.uint8] | None:
         if not self.video_cli.wait_for_service(timeout_sec=1.0):
             self.get_logger().error("Video capture service not available, exiting...")
@@ -99,6 +108,8 @@ def done_callback(future):
         if image is None or image.size == 0:
             self.get_logger().error("Failed to decode image")
             return None
+        # Hack: The bottom of our video stream is sometimes dirty. Remove it
+        image = self.crop_bottom(image, 0.10)
         return image
 
     def construct_panoramic(self, images):

src/Teleop-Control/joystick_control/config/3dpro.yaml

@@ -15,6 +15,7 @@ drive_teleop_node:
     servo_max: 6.28
     default_servo_x: 3.10
     default_servo_y: 3.75
+    default_servo_m: 4.2
 
 arm_teleop_node:
   ros__parameters:

src/Teleop-Control/joystick_control/config/pxn.yaml

@@ -15,6 +15,7 @@ drive_teleop_node:
     servo_max: 6.28
     default_servo_x: 3.10
     default_servo_y: 3.75
+    default_servo_m: 4.2
 
 arm_teleop_node:
   ros__parameters:

src/Teleop-Control/joystick_control/include/drive.hpp

@@ -3,11 +3,11 @@
 
 #include "geometry_msgs/msg/twist.hpp"
 #include "interfaces/msg/video_preset.hpp"
-#include "interfaces/srv/get_presets.hpp"
 #include "interfaces/srv/video_out.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "sensor_msgs/msg/joy.hpp"
 #include "std_msgs/msg/float32.hpp"
+#include <interfaces/msg/video_presets.hpp>
 
 class drive : public rclcpp::Node {
 public:
@@ -20,15 +20,17 @@ class drive : public rclcpp::Node {
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr servo_x_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr servo_m_pub_;
   rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr drive_throttle_sub_;
+  rclcpp::Subscription<interfaces::msg::VideoPresets>::SharedPtr presets_sub_;
   rclcpp::Client<interfaces::srv::VideoOut>::SharedPtr camera_client_;
-  rclcpp::Client<interfaces::srv::GetPresets>::SharedPtr list_presets_client_;
   std::vector<interfaces::msg::VideoPreset> video_carousell_;
   size_t video_carousell_idx_;
 
   void camera_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg);
+  void drive_throttle_cb(const std_msgs::msg::Float32::SharedPtr throttle_msg);
   void declare_parameters();
   void load_parameters();
-  void setCarousell();
+  void presets_cb(const interfaces::msg::VideoPresets::SharedPtr presets_msg);
 
   // Parameters
   double kMaxLinear;
@@ -41,6 +43,7 @@ class drive : public rclcpp::Node {
   int kServoHomeButton;
   int kMastLeftButton;
   int kMastRightButton;
+  int kLockTurnBut;
   int kCamLeftBut;
   int kCamRightBut;
   double kServoIncrement;
@@ -49,11 +52,13 @@ class drive : public rclcpp::Node {
   double kJoyDeadzone;
   double kDefaultServoX;
   double kDefaultServoY;
+  double kDefaultServoM;
 
   bool initialized_;
   double servo_y_;
   double servo_x_;
   double servo_mast_;
+  float drive_throttle_;
 
   bool cam_debounce_;
 };

src/Teleop-Control/joystick_control/src/arm_teleop.cpp

@@ -32,14 +32,14 @@ ArmTeleop::ArmTeleop(const rclcpp::NodeOptions &options)
       "~/state", rclcpp::QoS(1).reliable().transient_local());
   go_to_named_pose_client_ =
       this->create_client<interfaces::srv::GoToNamedPose>(
-          "/move_group_client/go_to_named_pose");
+          "/move_group_interface/go_to_named_pose");
   save_current_pose_client_ =
       this->create_client<interfaces::srv::SaveCurrentPose>(
-          "/move_group_client/save_current_pose");
+          "/move_group_interface/save_current_pose");
   go_to_cam_coord_client_ = this->create_client<interfaces::srv::GoToCamCoord>(
-      "/move_group_client/go_to_cam_coord");
+      "/move_group_interface/go_to_cam_coord");
   stop_move_group_client_ =
-      this->create_client<std_srvs::srv::Trigger>("/move_group_client/stop");
+      this->create_client<std_srvs::srv::Trigger>("/move_group_interface/stop");
   servo_input_client_ = this->create_client<moveit_msgs::srv::ServoCommandType>(
       "/servo_node/switch_command_type");
   clear_dot();

src/Teleop-Control/joystick_control/src/drive.cpp

@@ -11,32 +11,42 @@ drive::drive() : Node("drive_node"), initialized_(false) {
 
   camera_client_ =
       this->create_client<interfaces::srv::VideoOut>("/start_video");
-  list_presets_client_ =
-      this->create_client<interfaces::srv::GetPresets>("/list_presets");
+
+  auto presets_qos =
+      rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local();
+
+  presets_sub_ = this->create_subscription<interfaces::msg::VideoPresets>(
+      "/video_presets", presets_qos,
+      std::bind(&drive::presets_cb, this, std::placeholders::_1));
+
   joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
       "/joy", rclcpp::QoS(2).best_effort(),
       std::bind(&drive::drive_control, this, std::placeholders::_1));
+  drive_throttle_sub_ = this->create_subscription<std_msgs::msg::Float32>(
+      "/drive_throttle", 10,
+      std::bind(&drive::drive_throttle_cb, this, std::placeholders::_1));
   RCLCPP_INFO(this->get_logger(), "Drive controller started");
   servo_y_ = kDefaultServoY;
   servo_x_ = kDefaultServoX;
-  servo_mast_ = 0;
-  setCarousell();
+  servo_mast_ = kDefaultServoM;
+  drive_throttle_ = 1.0f;
 };
-void drive::setCarousell() {
-  if (!list_presets_client_->wait_for_service(std::chrono::seconds(2))) {
-    RCLCPP_ERROR(this->get_logger(), "List presets service is unavailable!!!!");
-    return;
-  }
 
-  auto request = std::make_shared<interfaces::srv::GetPresets::Request>();
+void drive::presets_cb(
+    const interfaces::msg::VideoPresets::SharedPtr presets_msg) {
+  video_carousell_ = presets_msg->presets;
+  video_carousell_idx_ = 0;
 
-  list_presets_client_->async_send_request(
-      request,
-      [this](rclcpp::Client<interfaces::srv::GetPresets>::SharedFuture msg) {
-        auto response = msg.get();
-        video_carousell_ = std::move(response->presets);
-        video_carousell_idx_ = 0;
-      });
+  RCLCPP_INFO(this->get_logger(), "Received %zu video presets",
+              video_carousell_.size());
+}
+
+void drive::drive_throttle_cb(
+    const std_msgs::msg::Float32::SharedPtr throttle_msg) {
+  drive_throttle_ = std::clamp(throttle_msg->data, 0.0f, 1.0f);
+
+  RCLCPP_INFO(this->get_logger(), "Drive throttle set to %.2f",
+              drive_throttle_);
 }
 
 void drive::camera_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
@@ -52,14 +62,20 @@ void drive::camera_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
     return;
   }
   cam_debounce_ = true;
+
+  if (video_carousell_.empty()) {
+    RCLCPP_WARN(this->get_logger(), "No video presets have been received");
+    return;
+  }
+
   if (left_but) {
     if (video_carousell_idx_ == 0) {
       video_carousell_idx_ = video_carousell_.size();
     }
     --video_carousell_idx_;
   }
   if (right_but) {
-    if (++video_carousell_idx_ == video_carousell_.size()) {
+    if (++video_carousell_idx_ >= video_carousell_.size()) {
       video_carousell_idx_ = 0;
     }
   }
@@ -80,16 +96,25 @@ void drive::drive_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
     return;
   }
   auto twist = geometry_msgs::msg::Twist();
-  twist.linear.x = joystickMsg->axes[kForwardAxis] * kMaxLinear;
-  twist.linear.y = joystickMsg->axes[kStrafeAxis] * kMaxLinear;
-  twist.angular.z = joystickMsg->axes[kYawAxis] * kMaxAngular;
+  twist.linear.x =
+      joystickMsg->axes[kForwardAxis] * kMaxLinear * drive_throttle_;
+  twist.linear.y =
+      joystickMsg->axes[kStrafeAxis] * kMaxLinear * drive_throttle_;
+  twist.angular.z = joystickMsg->axes[kYawAxis] * kMaxAngular * drive_throttle_;
+
+  if (joystickMsg->buttons[kLockTurnBut]) {
+    twist.angular.z = 0.0;
+    twist.linear.y = 0.0;
+  }
 
   twist_pub_->publish(twist);
   camera_control(joystickMsg);
-
+  bool pub_mast = false;
   if (joystickMsg->buttons[kServoHomeButton]) {
     servo_y_ = kDefaultServoY;
     servo_x_ = kDefaultServoX;
+    servo_mast_ = kDefaultServoM;
+    pub_mast = true;
   }
 
   if (joystickMsg->axes[kServoYAxis] > kJoyDeadzone) {
@@ -99,8 +124,10 @@ void drive::drive_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
   }
   if (joystickMsg->buttons[kMastLeftButton]) {
     servo_mast_ -= kServoIncrement;
+    pub_mast = true;
   } else if (joystickMsg->buttons[kMastRightButton]) {
     servo_mast_ += kServoIncrement;
+    pub_mast = true;
   }
   if (joystickMsg->axes[kServoXAxis] > kJoyDeadzone) {
     servo_x_ += kServoIncrement;
@@ -115,10 +142,11 @@ void drive::drive_control(std::shared_ptr<sensor_msgs::msg::Joy> joystickMsg) {
   servo_x_ = std::clamp(servo_x_, kServoMin, kServoMax);
   servo_msg.data = servo_x_;
   servo_x_pub_->publish(servo_msg);
-
-  servo_mast_ = std::clamp(servo_mast_, kServoMin, kServoMax);
-  servo_msg.data = servo_mast_;
-  servo_m_pub_->publish(servo_msg);
+  if (pub_mast) {
+    servo_mast_ = std::clamp(servo_mast_, kServoMin, kServoMax);
+    servo_msg.data = servo_mast_;
+    servo_m_pub_->publish(servo_msg);
+  }
 };
 
 void drive::declare_parameters() {
@@ -134,13 +162,16 @@ void drive::declare_parameters() {
   this->declare_parameter("mast_right_button", 1);
   this->declare_parameter("cam_left_button", 4);
   this->declare_parameter("cam_right_button", 5);
+  this->declare_parameter("lock_turn_button", 6);
   this->declare_parameter("servo_increment", 0.01);
   this->declare_parameter("servo_min", -3.14);
   this->declare_parameter("servo_max", 3.14);
   this->declare_parameter("joy_deadzone", 0.01);
   this->declare_parameter("default_servo_x", 0.0);
   this->declare_parameter("default_servo_y", 0.0);
+  this->declare_parameter("default_servo_m", 0.0);
 }
+
 void drive::load_parameters() {
   this->get_parameter("max_linear", kMaxLinear);
   this->get_parameter("max_angular", kMaxAngular);
@@ -154,12 +185,14 @@ void drive::load_parameters() {
   this->get_parameter("mast_right_button", kMastRightButton);
   this->get_parameter("cam_left_button", kCamLeftBut);
   this->get_parameter("cam_right_button", kCamRightBut);
+  this->get_parameter("lock_turn_button", kLockTurnBut);
   this->get_parameter("servo_increment", kServoIncrement);
   this->get_parameter("servo_min", kServoMin);
   this->get_parameter("servo_max", kServoMax);
   this->get_parameter("joy_deadzone", kJoyDeadzone);
   this->get_parameter("default_servo_x", kDefaultServoX);
   this->get_parameter("default_servo_y", kDefaultServoY);
+  this->get_parameter("default_servo_m", kDefaultServoM);
 
   RCLCPP_INFO(this->get_logger(), "Loaded Max Linear: %f", kMaxLinear);
   RCLCPP_INFO(this->get_logger(), "Loaded Max Angular: %f", kMaxAngular);