diff --git a/setup/start_rover.sh b/setup/start_rover.sh index 745a299c..e47582ff 100755 --- a/setup/start_rover.sh +++ b/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 diff --git a/src/Bringup/launch/websocket.launch.py b/src/Bringup/launch/websocket.launch.py new file mode 100644 index 00000000..1ec42134 --- /dev/null +++ b/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 diff --git a/src/Cameras/video_streaming/config/presets.yaml b/src/Cameras/video_streaming/config/presets.yaml index cf0edc8e..ad32ee4f 100644 --- a/src/Cameras/video_streaming/config/presets.yaml +++ b/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 \ No newline at end of file diff --git a/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp b/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp index e78c3c51..ed2360e0 100644 --- a/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp +++ b/src/Cameras/video_streaming/include/video_streaming/preset_node.hpp @@ -4,7 +4,7 @@ #include #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 presets_; - rclcpp::Service::SharedPtr list_presets_service_; + rclcpp::Publisher::SharedPtr presets_pub_; }; #endif // PRESET_NODE_HPP \ No newline at end of file diff --git a/src/Cameras/video_streaming/src/preset_node.cpp b/src/Cameras/video_streaming/src/preset_node.cpp index 742ad915..264860c2 100644 --- a/src/Cameras/video_streaming/src/preset_node.cpp +++ b/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( - "/list_presets", - [this]( - const std::shared_ptr request, - std::shared_ptr response) { - response->presets = presets_; - }); + auto presets_qos = + rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(); + + presets_pub_ = this->create_publisher( + "/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"); } diff --git a/src/HW-Devices/science_sensors/science_sensors/panoramic.py b/src/HW-Devices/science_sensors/science_sensors/panoramic.py index 1b7bb2a5..3f49ba4e 100644 --- a/src/HW-Devices/science_sensors/science_sensors/panoramic.py +++ b/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): diff --git a/src/Teleop-Control/joystick_control/config/3dpro.yaml b/src/Teleop-Control/joystick_control/config/3dpro.yaml index 44246122..e3e75fa8 100644 --- a/src/Teleop-Control/joystick_control/config/3dpro.yaml +++ b/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: diff --git a/src/Teleop-Control/joystick_control/config/pxn.yaml b/src/Teleop-Control/joystick_control/config/pxn.yaml index 5a7b1abb..99aac4cc 100644 --- a/src/Teleop-Control/joystick_control/config/pxn.yaml +++ b/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: diff --git a/src/Teleop-Control/joystick_control/include/drive.hpp b/src/Teleop-Control/joystick_control/include/drive.hpp index 9ff5b52b..d3c05cfe 100644 --- a/src/Teleop-Control/joystick_control/include/drive.hpp +++ b/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 class drive : public rclcpp::Node { public: @@ -20,15 +20,17 @@ class drive : public rclcpp::Node { rclcpp::Publisher::SharedPtr servo_x_pub_; rclcpp::Publisher::SharedPtr servo_m_pub_; rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Subscription::SharedPtr drive_throttle_sub_; + rclcpp::Subscription::SharedPtr presets_sub_; rclcpp::Client::SharedPtr camera_client_; - rclcpp::Client::SharedPtr list_presets_client_; std::vector video_carousell_; size_t video_carousell_idx_; void camera_control(std::shared_ptr 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_; }; diff --git a/src/Teleop-Control/joystick_control/src/arm_teleop.cpp b/src/Teleop-Control/joystick_control/src/arm_teleop.cpp index c375b8ce..c6db5c06 100644 --- a/src/Teleop-Control/joystick_control/src/arm_teleop.cpp +++ b/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( - "/move_group_client/go_to_named_pose"); + "/move_group_interface/go_to_named_pose"); save_current_pose_client_ = this->create_client( - "/move_group_client/save_current_pose"); + "/move_group_interface/save_current_pose"); go_to_cam_coord_client_ = this->create_client( - "/move_group_client/go_to_cam_coord"); + "/move_group_interface/go_to_cam_coord"); stop_move_group_client_ = - this->create_client("/move_group_client/stop"); + this->create_client("/move_group_interface/stop"); servo_input_client_ = this->create_client( "/servo_node/switch_command_type"); clear_dot(); diff --git a/src/Teleop-Control/joystick_control/src/drive.cpp b/src/Teleop-Control/joystick_control/src/drive.cpp index 7b9fad4b..c4fb75d6 100644 --- a/src/Teleop-Control/joystick_control/src/drive.cpp +++ b/src/Teleop-Control/joystick_control/src/drive.cpp @@ -11,32 +11,42 @@ drive::drive() : Node("drive_node"), initialized_(false) { camera_client_ = this->create_client("/start_video"); - list_presets_client_ = - this->create_client("/list_presets"); + + auto presets_qos = + rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(); + + presets_sub_ = this->create_subscription( + "/video_presets", presets_qos, + std::bind(&drive::presets_cb, this, std::placeholders::_1)); + joy_sub_ = this->create_subscription( "/joy", rclcpp::QoS(2).best_effort(), std::bind(&drive::drive_control, this, std::placeholders::_1)); + drive_throttle_sub_ = this->create_subscription( + "/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(); +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::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 joystickMsg) { @@ -52,6 +62,12 @@ void drive::camera_control(std::shared_ptr 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(); @@ -59,7 +75,7 @@ void drive::camera_control(std::shared_ptr joystickMsg) { --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 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 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 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); diff --git a/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp index 8c5cd904..ba774d4b 100644 --- a/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp +++ b/src/Teleop-Control/system-telemetry-cpp/include/system-telemetry-cpp/node_status_publisher.hpp @@ -1,14 +1,13 @@ #ifndef NODE_STATUS_PUBLISHER_HPP #define NODE_STATUS_PUBLISHER_HPP +#include +#include #include -#include #include #include -#include "interfaces/msg/node_list.hpp" - class NodeStatusPublisher : public rclcpp::Node { public: NodeStatusPublisher(); @@ -16,9 +15,14 @@ class NodeStatusPublisher : public rclcpp::Node { private: void publish_nodes(); + void + get_topics(const std::shared_ptr request, + std::shared_ptr response); + double frequency_; rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Service::SharedPtr topics_service_; rclcpp::TimerBase::SharedPtr timer_; }; diff --git a/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp b/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp index 21117ee9..aaaf259f 100644 --- a/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp +++ b/src/Teleop-Control/system-telemetry-cpp/src/node_status_publisher.cpp @@ -14,6 +14,11 @@ NodeStatusPublisher::NodeStatusPublisher() publisher_ = this->create_publisher("/system/nodes", qos); + topics_service_ = this->create_service( + "/system/get_topics", + std::bind(&NodeStatusPublisher::get_topics, this, std::placeholders::_1, + std::placeholders::_2)); + const auto period = std::chrono::duration(1.0 / frequency_); timer_ = this->create_wall_timer( std::chrono::duration_cast(period), @@ -29,6 +34,21 @@ void NodeStatusPublisher::publish_nodes() { publisher_->publish(msg); } +void NodeStatusPublisher::get_topics( + const std::shared_ptr request, + std::shared_ptr response) { + (void)request; + + const auto topics_and_types = this->get_topic_names_and_types(); + + for (const auto &[topic, types] : topics_and_types) { + for (const auto &type : types) { + response->topics.push_back(topic); + response->types.push_back(type); + } + } +} + int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); diff --git a/src/interfaces/CMakeLists.txt b/src/interfaces/CMakeLists.txt index 4acd4fcc..f8d5d9e6 100644 --- a/src/interfaces/CMakeLists.txt +++ b/src/interfaces/CMakeLists.txt @@ -33,11 +33,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/SvinStatus.msg" "msg/SystemTelemetry.msg" "msg/VideoPreset.msg" + "msg/VideoPresets.msg" "msg/VideoSource.msg" "srv/FilterGoals.srv" "srv/GetCameras.srv" "srv/GetNamedTargets.srv" - "srv/GetPresets.srv" + "srv/GetTopics.srv" "srv/GoToCamCoord.srv" "srv/GoToNamedPose.srv" "srv/GoToPose.srv" diff --git a/src/interfaces/msg/VideoPresets.msg b/src/interfaces/msg/VideoPresets.msg new file mode 100644 index 00000000..1aeafab5 --- /dev/null +++ b/src/interfaces/msg/VideoPresets.msg @@ -0,0 +1,3 @@ +# interfaces/msg/VideoPresets.msg + +VideoPreset[] presets \ No newline at end of file diff --git a/src/interfaces/srv/GetPresets.srv b/src/interfaces/srv/GetPresets.srv deleted file mode 100644 index 006d2802..00000000 --- a/src/interfaces/srv/GetPresets.srv +++ /dev/null @@ -1,4 +0,0 @@ -# GetPresets.srv - ---- -VideoPreset[] presets \ No newline at end of file diff --git a/src/interfaces/srv/GetTopics.srv b/src/interfaces/srv/GetTopics.srv new file mode 100644 index 00000000..6b30219b --- /dev/null +++ b/src/interfaces/srv/GetTopics.srv @@ -0,0 +1,4 @@ +# interfaces/srv/GetTopics.srv +--- +string[] topics +string[] types \ No newline at end of file