From d3f908c9d39b3bfd6476962253e49d640f2e338f Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 17 May 2023 09:10:49 -0700 Subject: [PATCH 1/5] Ported angler_localization --- blue_bringup/config/blue.yaml | 37 +++ blue_bringup/launch/blue.launch.py | 35 ++ blue_control/CMakeLists.txt | 2 +- .../{base_controller.hpp => controller.hpp} | 6 +- blue_control/include/blue_control/ismc.hpp | 4 +- .../{base_controller.cpp => controller.cpp} | 10 +- blue_control/src/ismc.cpp | 4 +- blue_localization/LICENSE | 17 + .../blue_localization/__init__.py | 0 .../blue_localization/localizer.py | 300 ++++++++++++++++++ blue_localization/blue_localization/source.py | 300 ++++++++++++++++++ .../launch/localization.launch.py | 146 +++++++++ blue_localization/launch/markers.launch.py | 58 ++++ blue_localization/package.xml | 35 ++ blue_localization/resource/blue_localization | 0 blue_localization/setup.cfg | 4 + blue_localization/setup.py | 55 ++++ blue_localization/test/test_copyright.py | 27 ++ blue_localization/test/test_flake8.py | 25 ++ 19 files changed, 1052 insertions(+), 13 deletions(-) rename blue_control/include/blue_control/{base_controller.hpp => controller.hpp} (97%) rename blue_control/src/{base_controller.cpp => controller.cpp} (97%) create mode 100644 blue_localization/LICENSE create mode 100644 blue_localization/blue_localization/__init__.py create mode 100644 blue_localization/blue_localization/localizer.py create mode 100644 blue_localization/blue_localization/source.py create mode 100644 blue_localization/launch/localization.launch.py create mode 100644 blue_localization/launch/markers.launch.py create mode 100644 blue_localization/package.xml create mode 100644 blue_localization/resource/blue_localization create mode 100644 blue_localization/setup.cfg create mode 100644 blue_localization/setup.py create mode 100644 blue_localization/test/test_copyright.py create mode 100644 blue_localization/test/test_flake8.py diff --git a/blue_bringup/config/blue.yaml b/blue_bringup/config/blue.yaml index 3ab21c78..e086f979 100644 --- a/blue_bringup/config/blue.yaml +++ b/blue_bringup/config/blue.yaml @@ -26,6 +26,43 @@ ismc: msg_ids: [31, 32] msg_rates: [100.0, 100.0] +aruco_marker_localizer: + ros__parameters: + camera_matrix: + [1078.17559, 0.0, 1010.57086, 0.0, 1076.46176, 463.06243, 0.0, 0.0, 1.0] + distortion_coefficients: + [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] + projection_matrix: + [ + 1108.25366, + 0.0, + 1003.75555, + 0.0, + 0.0, + 1108.39001, + 456.92861, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + ] + +camera: + ros__parameters: + port: 5600 + +qualisys_mocap: + ros__parameters: + ip: "192.168.0.0" + port: 22223 + version: "1.22" + body: "angler" + +qualisys_localizer: + ros__parameters: + body: "angler" # This should be the same as the body parameter setting for the qualisys_mocap node + mavros: ros__parameters: system_id: 255 diff --git a/blue_bringup/launch/blue.launch.py b/blue_bringup/launch/blue.launch.py index 5c1cbde7..b041e5a8 100644 --- a/blue_bringup/launch/blue.launch.py +++ b/blue_bringup/launch/blue.launch.py @@ -48,6 +48,28 @@ def generate_launch_description() -> LaunchDescription: ), choices=["ismc"], ), + DeclareLaunchArgument( + "localization_source", + default_value="mocap", + choices=["mocap", "camera"], + description="The localization source to stream from.", + ), + DeclareLaunchArgument( + "use_camera", + default_value="false", + description=( + "Launch the BlueROV2 camera stream. This is automatically set to true" + " when using the camera for localization." + ), + ), + DeclareLaunchArgument( + "use_mocap", + default_value="false", + description=( + "Launch the Qualisys motion capture stream. This is automatically" + " set to true when using the motion capture system for localization." + ), + ), ] config_filepath = PathJoinSubstitution( @@ -88,6 +110,19 @@ def generate_launch_description() -> LaunchDescription: "controller": LaunchConfiguration("controller"), }.items(), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_localization"), "localization.launch.py"] + ) + ), + launch_arguments={ + "config_filepath": config_filepath, + "localization_source": LaunchConfiguration("localization_source"), + "use_mocap": LaunchConfiguration("use_mocap"), + "use_camera": LaunchConfiguration("use_camera"), + }.items(), + ), ] return LaunchDescription(args + nodes + includes) diff --git a/blue_control/CMakeLists.txt b/blue_control/CMakeLists.txt index e2b77e42..96c22bb6 100644 --- a/blue_control/CMakeLists.txt +++ b/blue_control/CMakeLists.txt @@ -45,7 +45,7 @@ include_directories( ) # Create a library for the base_controller class -add_library(base_controller include/blue_control/base_controller.hpp src/base_controller.cpp) +add_library(base_controller include/blue_control/controller.hpp src/controller.cpp) ament_target_dependencies(base_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Create a library for the custom controllers; link this to the base controller diff --git a/blue_control/include/blue_control/base_controller.hpp b/blue_control/include/blue_control/controller.hpp similarity index 97% rename from blue_control/include/blue_control/base_controller.hpp rename to blue_control/include/blue_control/controller.hpp index 2b68550e..7cc40634 100644 --- a/blue_control/include/blue_control/base_controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -66,15 +66,15 @@ Eigen::MatrixXd convertVectorToEigenMatrix( /** * @brief A base class for custom BlueROV2 controllers. */ -class BaseController : public rclcpp::Node +class Controller : public rclcpp::Node { public: /** - * @brief Construct a new BaseController object. + * @brief Construct a new Controller object. * * @param node_name The name of the ROS node. */ - explicit BaseController(const std::string & node_name); + explicit Controller(const std::string & node_name); protected: /** diff --git a/blue_control/include/blue_control/ismc.hpp b/blue_control/include/blue_control/ismc.hpp index 40b8838b..70f0bbcd 100644 --- a/blue_control/include/blue_control/ismc.hpp +++ b/blue_control/include/blue_control/ismc.hpp @@ -22,7 +22,7 @@ #include -#include "blue_control/base_controller.hpp" +#include "blue_control/controller.hpp" #include "blue_msgs/msg/reference.hpp" #include "mavros_msgs/msg/override_rc_in.hpp" @@ -32,7 +32,7 @@ namespace blue::control /** * @brief Integral sliding mode controller for the BlueROV2. */ -class ISMC : public BaseController +class ISMC : public Controller { public: /** diff --git a/blue_control/src/base_controller.cpp b/blue_control/src/controller.cpp similarity index 97% rename from blue_control/src/base_controller.cpp rename to blue_control/src/controller.cpp index 79c7b144..a030e65f 100644 --- a/blue_control/src/base_controller.cpp +++ b/blue_control/src/controller.cpp @@ -18,7 +18,7 @@ // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // THE SOFTWARE. -#include "blue_control/base_controller.hpp" +#include "blue_control/controller.hpp" namespace blue::control { @@ -38,7 +38,7 @@ Eigen::MatrixXd convertVectorToEigenMatrix( return mat; } -BaseController::BaseController(const std::string & node_name) +Controller::Controller(const std::string & node_name) : Node(std::move(node_name)), armed_(false) { @@ -159,7 +159,7 @@ BaseController::BaseController(const std::string & node_name) }); } -void BaseController::armControllerCb( +void Controller::armControllerCb( const std::shared_ptr request, std::shared_ptr response) { @@ -178,7 +178,7 @@ void BaseController::armControllerCb( } } -void BaseController::setMessageRates( +void Controller::setMessageRates( const std::vector & msg_ids, const std::vector & rates) { // Check that the message IDs and rates are the same length @@ -196,7 +196,7 @@ void BaseController::setMessageRates( } } -void BaseController::setMessageRate(int64_t msg_id, float rate) +void Controller::setMessageRate(int64_t msg_id, float rate) { auto request = std::make_shared(); diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 9c3f46d2..37fad2f5 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -29,12 +29,12 @@ namespace blue::control { ISMC::ISMC() -: BaseController("ismc") +: Controller("ismc") { // Declare the ROS parameters specific to this controller // There are additional parameters defined by the base controller as well this->declare_parameter( - "convergence_rate", std::vector{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}); + "convergence_rate", std::vector({100.0, 100.0, 100.0, 100.0, 100.0, 100.0})); this->declare_parameter("sliding_gain", 0.0); this->declare_parameter("boundary_thickness", 0.0); diff --git a/blue_localization/LICENSE b/blue_localization/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/blue_localization/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/blue_localization/blue_localization/__init__.py b/blue_localization/blue_localization/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py new file mode 100644 index 00000000..9ece1a5d --- /dev/null +++ b/blue_localization/blue_localization/localizer.py @@ -0,0 +1,300 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from abc import ABC +from typing import Any + +import cv2 +import numpy as np +import rclpy +import tf2_geometry_msgs # noqa +import tf_transformations as tf +from cv_bridge import CvBridge +from geometry_msgs.msg import PoseStamped +from rclpy.node import Node +from sensor_msgs.msg import Image +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + + +class Localizer(Node, ABC): + """Base class for implementing a visual localization interface.""" + + def __init__(self, node_name: str) -> None: + """Create a new localizer. + + Args: + node_name: The name of the ROS 2 node. + """ + Node.__init__(self, node_name) + ABC.__init__(self) + + # Provide access to TF2 + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # SLAM poses are sent to the ArduPilot EKF + self.localization_pub = self.create_publisher( + PoseStamped, "/mavros/vision_pose/pose", 1 + ) + + +class ArucoMarkerLocalizer(Localizer): + """Performs localization using ArUco markers.""" + + ARUCO_MARKER_TYPES = [ + cv2.aruco.DICT_4X4_50, + cv2.aruco.DICT_4X4_100, + cv2.aruco.DICT_4X4_250, + cv2.aruco.DICT_4X4_1000, + cv2.aruco.DICT_5X5_50, + cv2.aruco.DICT_5X5_100, + cv2.aruco.DICT_5X5_250, + cv2.aruco.DICT_5X5_1000, + cv2.aruco.DICT_6X6_50, + cv2.aruco.DICT_6X6_100, + cv2.aruco.DICT_6X6_250, + cv2.aruco.DICT_6X6_1000, + cv2.aruco.DICT_7X7_50, + cv2.aruco.DICT_7X7_100, + cv2.aruco.DICT_7X7_250, + cv2.aruco.DICT_7X7_1000, + cv2.aruco.DICT_ARUCO_ORIGINAL, + ] + + def __init__(self) -> None: + """Create a new ArUco marker localizer.""" + super().__init__("aruco_marker_localizer") + + self.bridge = CvBridge() + + self.declare_parameters( + "", + [ + ("camera_matrix", [0.0 for _ in range(9)]), # Reshaped to 3x3 + ("projection_matrix", [0.0 for _ in range(12)]), # Reshaped to 3x4 + ("distortion_coefficients", [0.0 for _ in range(5)]), + ], + ) + + # Get the camera intrinsics + self.camera_matrix = np.array( + self.get_parameter("camera_matrix") + .get_parameter_value() + .double_array_value, + np.float32, + ).reshape(3, 3) + + self.projection_matrix = np.array( + self.get_parameter("projection_matrix") + .get_parameter_value() + .double_array_value, + np.float32, + ).reshape(3, 4) + + self.distortion_coefficients = np.array( + self.get_parameter("distortion_coefficients") + .get_parameter_value() + .double_array_value, + np.float32, + ).reshape(1, 5) + + self.camera_sub = self.create_subscription( + Image, "/blue/camera", self.extract_and_publish_pose_cb, 1 + ) + + def detect_markers(self, frame: np.ndarray) -> tuple[Any, Any] | None: + """Detect any ArUco markers in the frame. + + All markers in a frame should be the same type of ArUco marker + (e.g., 4x4 50) if multiple are expected to be in-frame. + + Args: + frame: The video frame containing ArUco markers. + + Returns: + A list of marker corners and IDs. If no markers were found, returns None. + """ + # Check each tag type, breaking when we find one that works + for tag_type in self.ARUCO_MARKER_TYPES: + aruco_dict = cv2.aruco.Dictionary_get(tag_type) + aruco_params = cv2.aruco.DetectorParameters_create() + + try: + # Return the corners and ids if we find the correct tag type + corners, ids, _ = cv2.aruco.detectMarkers( + frame, aruco_dict, parameters=aruco_params + ) + + if len(ids) > 0: + return corners, ids + + except Exception: + continue + + # Nothing was found + return None + + def get_camera_pose(self, frame: np.ndarray) -> tuple[Any, Any, int] | None: + """Get the pose of the camera relative to any ArUco markers detected. + + If multiple markers are detected, then the "largest" marker will be used to + determine the pose of the camera. + + Args: + frame: The camera frame containing ArUco markers. + + Returns: + The rotation vector and translation vector of the camera in the marker + frame and the ID of the marker detected. If no marker was detected, + returns None. + """ + # Convert to greyscale image then try to detect the tag(s) + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + detection = self.detect_markers(gray) + + if detection is None: + return None + + corners, ids = detection + + # If there are multiple markers, get the marker with the "longest" side, where + # "longest" should be interpreted as the relative size in the image + side_lengths = [ + abs(corner[0][0][0] - corner[0][2][0]) + + abs(corner[0][0][1] - corner[0][2][1]) + for corner in corners + ] + + min_side_idx = side_lengths.index(max(side_lengths)) + min_marker_id = ids[min_side_idx] + + # Get the estimated pose + rot_vec, trans_vec, _ = cv2.aruco.estimatePoseSingleMarkers( + corners[min_side_idx], + min_marker_id, + self.camera_matrix, + self.distortion_coefficients, + ) + + return rot_vec, trans_vec, min_marker_id + + def extract_and_publish_pose_cb(self, frame: Image) -> None: + """Get the camera pose relative to the marker and send to the ArduSub EKF. + + Args: + frame: The BlueROV2 camera frame. + """ + # Get the pose of the camera in the `marker` frame + camera_pose = self.get_camera_pose(self.bridge.imgmsg_to_cv2(frame)) + + # If there was no marker in the image, exit early + if camera_pose is None: + self.get_logger().debug( + "An ArUco marker could not be detected in the current image" + ) + return + + rot_vec, trans_vec, marker_id = camera_pose + + # Convert the pose into a PoseStamped message + pose = PoseStamped() + + pose.header.frame_id = f"marker_{marker_id}" + pose.header.stamp = self.get_clock().now().to_msg() + + ( + pose.position.x, + pose.position.y, + pose.position.z, + ) = trans_vec.squeeze() + + tf_mat = np.identity(4) + tf_mat[:3, :3], _ = cv2.Rodrigues(rot_vec) + + ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) = tf.quaternion_from_matrix(tf_mat) + + # Transform the pose from the `marker` frame to the `map` frame + try: + pose = self.tf_buffer.transform(pose, "map") + except TransformException as e: + self.get_logger().warning( + f"Could not transform from frame marker_{marker_id} to map: {e}" + ) + return + + # Publish the transformed pose + self.localization_pub.publish(pose) + + +class QualisysLocalizer(Localizer): + """Localize the BlueROV2 using the Qualisys motion capture system.""" + + def __init__(self) -> None: + """Create a new Qualisys motion capture localizer.""" + super().__init__("qualisys_localizer") + + self.declare_parameter("body", "bluerov") + + body = self.get_parameter("body").get_parameter_value().string_value + + self.mocap_sub = self.create_subscription( + PoseStamped, f"/blue/mocap/qualisys/{body}", self.proxy_pose_cb, 1 + ) + + def proxy_pose_cb(self, pose: PoseStamped) -> None: + """Proxy the pose to the ArduSub EKF. + + The pose published by the Qualisys motion capture system is already defined in + the map frame. Therefore, all that needs to be done is to proxy this forward to + the EKF. + + Args: + pose: The pose of the BlueROV2 identified by the motion capture system. + """ + self.localization_pub.publish(pose) + + +def main_aruco(args: list[str] | None = None): + """Run the ArUco marker detector.""" + rclpy.init(args=args) + + node = ArucoMarkerLocalizer() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +def main_qualisys(args: list[str] | None = None): + """Run the Qualisys localizer.""" + rclpy.init(args=args) + + node = QualisysLocalizer() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() diff --git a/blue_localization/blue_localization/source.py b/blue_localization/blue_localization/source.py new file mode 100644 index 00000000..f8545763 --- /dev/null +++ b/blue_localization/blue_localization/source.py @@ -0,0 +1,300 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import asyncio +import xml.etree.ElementTree as ET +from abc import ABC +from typing import Any + +import gi +import numpy as np +import qtm +import rclpy +from cv_bridge import CvBridge +from geometry_msgs.msg import PoseStamped +from rclpy.node import Node +from scipy.spatial.transform import Rotation as R +from sensor_msgs.msg import Image + +gi.require_version("Gst", "1.0") +from gi.repository import Gst # noqa + + +class Source(Node, ABC): + """Base class for a localization source (e.g., camera, sonar, etc.).""" + + def __init__(self, node_name: str) -> None: + """Create a new localization source. + + Args: + node_name: The name of the ROS 2 node. + """ + Node.__init__(self, node_name) + ABC.__init__(self) + + +class Camera(Source): + """BlueROV2 camera source. + + The camera source uses GStreamer to proxy the BlueROV2 camera stream (i.e., frames + received from GStreamer are converted to ROS ``Image`` messages and republished for + other packages to use). + """ + + def __init__(self) -> None: + """Create a new Camera source.""" + super().__init__("camera") + + self.bridge = CvBridge() + + self.declare_parameter("port", 5600) + + self.camera_frame_pub = self.create_publisher(Image, "/blue/camera", 1) + + # Start the GStreamer stream + self.video_pipe, self.video_sink = self.init_stream( + self.get_parameter("port").get_parameter_value().integer_value + ) + + def init_stream(self, port: int) -> tuple[Any, Any]: + """Initialize a GStreamer video stream interface. + + GStreamer is used to receive video frames from the BlueROV2 for processing. + + Args: + port: The port over which the video feed is being streamed. + + Returns: + The video pipe and sink. + """ + Gst.init(None) + + video_source = f"udpsrc port={port}" + video_codec = ( + "! application/x-rtp, payload=96 ! rtph264depay ! h264parse ! avdec_h264" + ) + video_decode = ( + "! decodebin ! videoconvert ! video/x-raw,format=(string)BGR ! videoconvert" + ) + video_sink_conf = ( + "! appsink emit-signals=true sync=false max-buffers=2 drop=true" + ) + + command = " ".join([video_source, video_codec, video_decode, video_sink_conf]) + + video_pipe = Gst.parse_launch(command) + video_pipe.set_state(Gst.State.PLAYING) + + video_sink = video_pipe.get_by_name("appsink0") + + def proxy_frame_cb(sink: Any) -> Any: + # Convert from a GStreamer frame to a ROS 2 message and publish + frame = self.gst_to_opencv(sink.emit("pull-sample")) + self.camera_frame_pub.publish(self.bridge.cv2_to_imgmsg(frame)) + return Gst.FlowReturn.OK + + video_sink.connect("new-sample", proxy_frame_cb) + + return video_pipe, video_sink + + @staticmethod + def gst_to_opencv(frame: Any) -> np.ndarray: + """Convert a GStreamer frame to an array. + + Args: + frame: The GStreamer frame to convert. + + Returns: + The GStreamer video frame as an array. + """ + buf = frame.get_buffer() + caps = frame.get_caps() + + return np.ndarray( + ( + caps.get_structure(0).get_value("height"), + caps.get_structure(0).get_value("width"), + 3, + ), + buffer=buf.extract_dup(0, buf.get_size()), + dtype=np.uint8, + ) + + +class QualisysMotionCapture(Source): + """Qualisys motion capture system source. + + The Qualisys motion capture source provides an ROS 2 wrapper for the Qualisys + Python SDK. The source streams the pose of the bluerov body and republishes the pose + as a `PoseStamped` message. + """ + + def __init__(self) -> None: + """Create a new Qualisys motion capture source.""" + super().__init__("qualisys_mocap") + + self.declare_parameters( + "", + [ + ("ip", "192.168.0.0"), + ("port", 22223), + ("version", "1.22"), + ("body", "bluerov"), + ], + ) + + # Load the parameters + self.ip = self.get_parameter("ip").get_parameter_value().string_value + self.port = self.get_parameter("port").get_parameter_value().integer_value + self.version = self.get_parameter("version").get_parameter_value().string_value + self.body = self.get_parameter("body").get_parameter_value().string_value + + # Publish the pose using the name of the body as the topic + self.mocap_pub = self.create_publisher( + PoseStamped, f"/blue/mocap/qualisys/{self.body}", 1 + ) + + @staticmethod + def create_body_index(params: str) -> dict[str, int]: + """Create a name to index dictionary from the 6-D parameters. + + This is used to retrieve the specific body of interest from a packet. + + Args: + params: The 6-D parameters to use for generating the mapping. + + Returns: + A mapping from the body name to its respective index in a packet. + """ + xml = ET.fromstring(params) + + body_to_index = {} + for index, body in enumerate(xml.findall("*/Body/Name")): + if body is not None: + body_to_index[body.text.strip()] = index # type: ignore + + return body_to_index + + async def run_mocap(self) -> None: + """Run the motion capture system coroutine. + + This implementation is inspired by the `stream_6dof_example.py` example included + with the Qualisys Python SDK. + """ + connection = await qtm.connect(self.ip, self.port, self.version) + + # Normally we would want to raise an exception here, but because the exception + # is returned as part of the future object--which doesn't get captured until + # keyboard interrupt due to the `spinning` method--we are going to notify + # the users with a log message and return. + if connection is None: + self.get_logger().error( + "The Qualisys motion capture source failed to establish a connection" + f" at the address '{self.ip}:{self.port}' using version" + f" '{self.version}'" + ) + return + + # Load the 6D parameters + params = await connection.get_parameters(parameters=["6d"]) + body_index = self.create_body_index(params) + + if self.body not in body_index: + self.get_logger().error( + f"The body '{self.body}' is not available. Please make sure that the" + " body has been properly defined and is enabled." + ) + return + + # Create a callback to bind to the frame stream + def proxy_pose_cb(packet: qtm.QRTPacket) -> None: + _, bodies = packet.get_6d_euler() + + position, rotation = bodies[body_index[self.body]] + + pose_msg = PoseStamped() + + pose_msg.header.frame_id = "map" + pose_msg.header.stamp = self.get_clock().now().to_msg() + + ( + pose_msg.pose.position.x, + pose_msg.pose.position.y, + pose_msg.pose.position.z, + ) = position + + ( + pose_msg.pose.orientation.x, + pose_msg.pose.orientation.y, + pose_msg.pose.orientation.z, + pose_msg.pose.orientation.w, + ) = R.from_euler("xyz", rotation).as_quat() + + self.mocap_pub.publish(pose_msg) + + # Stream the mocap 6D pose + await connection.stream_frames(components=["6d"], on_packet=proxy_pose_cb) + + +async def spinning(node: Node) -> None: + """Spin the ROS 2 node as an asyncio Task. + + Args: + node: The ROS 2 node to spin. + """ + while rclpy.ok(): + rclpy.spin_once(node, timeout_sec=0.01) + await asyncio.sleep(0.0001) + + +def main_camera(args: list[str] | None = None): + """Run the camera source.""" + rclpy.init(args=args) + + node = Camera() + rclpy.spin(node) + + node.destroy_node() + rclpy.shutdown() + + +def main_qualisys_mocap(args: list[str] | None = None): + """Run the Qualisys motion capture source.""" + + async def run_mocap(args: list[str] | None, loop: asyncio.AbstractEventLoop): + """Run the Qualisys motion capture system.""" + rclpy.init(args=args) + + node = QualisysMotionCapture() + + spin_task = loop.create_task(spinning(node)) + mocap_task = loop.create_task(node.run_mocap()) + + try: + await spin_task + except KeyboardInterrupt: + spin_task.cancel() + mocap_task.cancel() + + node.destroy_node() + rclpy.shutdown() + + loop = asyncio.new_event_loop() + loop.run_until_complete(run_mocap(args, loop)) diff --git a/blue_localization/launch/localization.launch.py b/blue_localization/launch/localization.launch.py new file mode 100644 index 00000000..c8143f8a --- /dev/null +++ b/blue_localization/launch/localization.launch.py @@ -0,0 +1,146 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the localization package. + + Returns: + The localization ROS 2 launch description. + """ + args = [ + DeclareLaunchArgument( + "config_filepath", + default_value=None, + description="The path to the configuration YAML file", + ), + DeclareLaunchArgument( + "localization_source", + default_value="mocap", + choices=["mocap", "camera"], + description="The localization source to stream from.", + ), + DeclareLaunchArgument( + "use_camera", + default_value="false", + description=( + "Launch the BlueROV2 camera stream. This is automatically set to true" + " when using the camera for localization." + ), + ), + DeclareLaunchArgument( + "use_mocap", + default_value="false", + description=( + "Launch the Qualisys motion capture stream. This is automatically" + " set to true when using the motion capture system for localization." + ), + ), + ] + + localization_source = LaunchConfiguration("localization_source") + use_camera = LaunchConfiguration("use_camera") + use_mocap = LaunchConfiguration("use_mocap") + + nodes = [ + Node( + package="blue_localization", + executable="camera", + name="camera", + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + condition=IfCondition( + PythonExpression( + [ + "'", + localization_source, + "' == 'camera' or '", + use_camera, + "' == 'true'", + ] + ) + ), + ), + Node( + package="blue_localization", + executable="aruco_marker_localizer", + name="aruco_marker_localizer", + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + condition=IfCondition( + PythonExpression(["'", localization_source, "' == 'camera'"]) + ), + ), + Node( + package="blue_localization", + executable="qualisys_mocap", + name="qualisys_mocap", + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + condition=IfCondition( + PythonExpression( + [ + "'", + localization_source, + "' == 'mocap' or '", + use_mocap, + "' == 'true'", + ] + ) + ), + ), + Node( + package="blue_localization", + executable="qualisys_localizer", + name="qualisys_localizer", + output="screen", + parameters=[LaunchConfiguration("config_filepath")], + condition=IfCondition( + PythonExpression(["'", localization_source, "' == 'mocap'"]) + ), + ), + ] + + includes = [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare("blue_localization"), "markers.launch.py"] + ) + ), + condition=IfCondition( + PythonExpression(["'", localization_source, "' == 'camera'"]) + ), + ) + ] + + return LaunchDescription(args + nodes + includes) diff --git a/blue_localization/launch/markers.launch.py b/blue_localization/launch/markers.launch.py new file mode 100644 index 00000000..0f54ba01 --- /dev/null +++ b/blue_localization/launch/markers.launch.py @@ -0,0 +1,58 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + """Generate a launch description for the ArUco marker TF broadcasters. + + Returns: + The ArUco marker TF broadcaster ROS 2 launch description. + """ + nodes = [ + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="marker_00_to_map_tf_broadcaster", + arguments=[ + "--x", + "0.0", + "--y", + "0.0", + "--z", + "0.0", + "--roll", + "0", + "--pitch", + "0", + "--yaw", + "0", + "--frame-id", + "map", + "--child-frame-id", + "marker_00", + ], + output="screen", + ) + ] + + return LaunchDescription(nodes) diff --git a/blue_localization/package.xml b/blue_localization/package.xml new file mode 100644 index 00000000..39bae0bb --- /dev/null +++ b/blue_localization/package.xml @@ -0,0 +1,35 @@ + + + + blue_localization + 0.0.1 + Localization interface used to provide visual odometry estimates to the ArduPilot + EKF. + Evan Palmer + MIT + + mavros + mavros_extras + tf_transformations + + rclpy + sensor_msgs + std_msgs + python3-opencv + python3-numpy + python3-transforms3d + mavros_msgs + tf2 + tf2_geometry_msgs + cv_bridge + tf2_ros + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/blue_localization/resource/blue_localization b/blue_localization/resource/blue_localization new file mode 100644 index 00000000..e69de29b diff --git a/blue_localization/setup.cfg b/blue_localization/setup.cfg new file mode 100644 index 00000000..09efd4b8 --- /dev/null +++ b/blue_localization/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/blue_localization +[install] +install_scripts=$base/lib/blue_localization diff --git a/blue_localization/setup.py b/blue_localization/setup.py new file mode 100644 index 00000000..ff7d8d1b --- /dev/null +++ b/blue_localization/setup.py @@ -0,0 +1,55 @@ +# Copyright 2023, Evan Palmer +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = "blue_localization" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (os.path.join("share", package_name), glob("launch/*.launch.py")), + ], + install_requires=["setuptools", "numpy", "transforms3d", "opencv-python", "qtm"], + zip_safe=True, + maintainer="Evan Palmer", + maintainer_email="evanp922@gmail.com", + description=( + "Localization interface used to provide visual odometry estimates to" + " the ArduPilot EKF." + ), + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "camera = blue_localization.source:main_camera", + "qualisys_mocap = blue_localization.source:main_qualisys_mocap", + "aruco_marker_localizer = blue_localization.localizer:main_aruco", + "qualisys_localizer = blue_localization.localizer:main_qualisys", + ], + }, +) diff --git a/blue_localization/test/test_copyright.py b/blue_localization/test/test_copyright.py new file mode 100644 index 00000000..8f18fa4b --- /dev/null +++ b/blue_localization/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_copyright.main import main + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/blue_localization/test/test_flake8.py b/blue_localization/test/test_flake8.py new file mode 100644 index 00000000..f494570f --- /dev/null +++ b/blue_localization/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest +from ament_flake8.main import main_with_errors + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) From 79dd63e70fb50d171d0c30c8140cee2836c38df5 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 17 May 2023 09:12:22 -0700 Subject: [PATCH 2/5] Added localization package to dockerignore --- .dockerignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.dockerignore b/.dockerignore index 3860a782..e5f60b97 100644 --- a/.dockerignore +++ b/.dockerignore @@ -7,3 +7,4 @@ !blue_control !blue_msgs !blue_bringup +!blue_localization From 2741c5d8fa0f6e55babce4896900da001a38a140 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 17 May 2023 16:58:12 -0700 Subject: [PATCH 3/5] Resolved controller bug and fixed dockerfile --- .docker/Dockerfile | 22 ++++++++++++++++++- blue_bringup/config/blue.yaml | 1 + .../include/blue_control/controller.hpp | 8 ++++++- blue_control/src/controller.cpp | 19 ++++++++++------ blue_control/src/ismc.cpp | 2 +- blue_localization/package.xml | 1 + 6 files changed, 43 insertions(+), 10 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 5bafb13c..584c4de8 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -21,11 +21,31 @@ RUN apt-get -q update \ clang-format-14 \ clang-tidy \ clang-tools \ + python3-pip \ software-properties-common \ && apt-get autoremove -y \ && apt-get clean -y \ && rm -rf /var/lib/apt/lists/* +# Install Qualisys Python SDK +RUN python3 -m pip install qtm + +# Install gstreamer +RUN apt-get -q update \ + && apt-get -q -y upgrade \ + && apt-get -q install --no-install-recommends -y \ + python3-gi \ + gstreamer1.0-tools \ + gir1.2-gstreamer-1.0 \ + gir1.2-gst-plugins-base-1.0 \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-ugly \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-libav \ + && apt-get autoremove -y \ + && apt-get clean -y \ + && rm -rf /var/lib/apt/lists/* + # Install all ROS dependencies RUN apt-get -q update \ && apt-get -q -y upgrade \ @@ -120,7 +140,7 @@ RUN apt-get -q update \ && rm -rf /var/lib/apt/lists/* # Install debugging/linting Python packages -RUN pip3 install \ +RUN python3 -m pip install \ pre-commit \ mypy \ isort \ diff --git a/blue_bringup/config/blue.yaml b/blue_bringup/config/blue.yaml index e086f979..b8335012 100644 --- a/blue_bringup/config/blue.yaml +++ b/blue_bringup/config/blue.yaml @@ -25,6 +25,7 @@ ismc: -0.1888, 0.1888, 0.1888, -0.1888, 0.0, 0.0, 0.0, 0.0] msg_ids: [31, 32] msg_rates: [100.0, 100.0] + control_loop_freq: 200.0 aruco_marker_localizer: ros__parameters: diff --git a/blue_control/include/blue_control/controller.hpp b/blue_control/include/blue_control/controller.hpp index 7cc40634..5ec410e1 100644 --- a/blue_control/include/blue_control/controller.hpp +++ b/blue_control/include/blue_control/controller.hpp @@ -102,7 +102,6 @@ class Controller : public rclcpp::Node */ sensor_msgs::msg::BatteryState battery_state_; - // /** * @brief The current pose and twist of the BlueROV2. * @@ -112,6 +111,13 @@ class Controller : public rclcpp::Node */ nav_msgs::msg::Odometry odom_; + /** + * @brief The total time (s) between control loop iterations + * + * @note This can be useful when calculating integral terms for the controller. + */ + double dt_{0.0}; + private: /** * @brief Enable the controller. diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index a030e65f..4c6760d6 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -59,6 +59,7 @@ Controller::Controller(const std::string & node_name) this->declare_parameter("num_thrusters", 8); this->declare_parameter("msg_ids", std::vector({31, 32})); this->declare_parameter("msg_rates", std::vector({100, 100})); + this->declare_parameter("control_loop_freq", 200.0); // I'm so sorry for this // You can blame the ROS devs for not supporting nested arrays for parameters @@ -149,14 +150,18 @@ Controller::Controller(const std::string & node_name) std::chrono::seconds(10), [this, msg_ids, msg_rates]() -> void { setMessageRates(msg_ids, msg_rates); }); + // Convert the control loop frequency to seconds + dt_ = 1 / this->get_parameter("control_loop_freq").as_double(); + // Run the controller at a rate of 200 Hz - // ArduSub only runs at a rate of 100 Hz, but we want to make sure to run the controller at - // a faster rate than the autopilot - control_loop_timer_ = this->create_wall_timer(std::chrono::milliseconds(5), [this]() -> void { - if (armed_) { - rc_override_pub_->publish(update()); - } - }); + // ArduSub only runs at a rate of 100 Hz, but we want to make sure to run the controller + // at a faster rate than the autopilot + control_loop_timer_ = + this->create_wall_timer(std::chrono::duration(dt_), [this]() -> void { + if (armed_) { + rc_override_pub_->publish(update()); + } + }); } void Controller::armControllerCb( diff --git a/blue_control/src/ismc.cpp b/blue_control/src/ismc.cpp index 37fad2f5..02d40214 100644 --- a/blue_control/src/ismc.cpp +++ b/blue_control/src/ismc.cpp @@ -73,7 +73,7 @@ mavros_msgs::msg::OverrideRCIn ISMC::update() // Make sure to update the velocity error integral term BEFORE calculating the sliding surface // (the integral is up to time "t") - total_velocity_error_ += velocity_error; + total_velocity_error_ += velocity_error * dt_; // Calculate the sliding surface Eigen::VectorXd surface = velocity_error + convergence_rate_ * total_velocity_error_; // NOLINT diff --git a/blue_localization/package.xml b/blue_localization/package.xml index 39bae0bb..c4130fa0 100644 --- a/blue_localization/package.xml +++ b/blue_localization/package.xml @@ -17,6 +17,7 @@ std_msgs python3-opencv python3-numpy + python3-scipy python3-transforms3d mavros_msgs tf2 From 4558858c5e23e4b8f99ebc107314a2a03dd8d616 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 17 May 2023 17:02:15 -0700 Subject: [PATCH 4/5] Removed redundant install --- .docker/Dockerfile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 584c4de8..3b53894e 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -22,6 +22,7 @@ RUN apt-get -q update \ clang-tidy \ clang-tools \ python3-pip \ + python3-dev \ software-properties-common \ && apt-get autoremove -y \ && apt-get clean -y \ @@ -130,8 +131,6 @@ RUN apt-get -q update \ RUN apt-get -q update \ && apt-get -q -y upgrade \ && apt-get -q install --no-install-recommends -y \ - python3-dev \ - python3-pip \ iputils-ping \ net-tools \ gdb \ From 13bcba692102e4d2599193b0a6594db63f7246d0 Mon Sep 17 00:00:00 2001 From: Evan Palmer Date: Wed, 17 May 2023 17:31:24 -0700 Subject: [PATCH 5/5] Resolved PR comments --- blue_bringup/config/blue.yaml | 25 ++++++------------- blue_bringup/test/test_pep257.py | 23 ----------------- blue_control/src/controller.cpp | 3 --- .../blue_localization/localizer.py | 2 +- 4 files changed, 9 insertions(+), 44 deletions(-) delete mode 100644 blue_bringup/test/test_pep257.py diff --git a/blue_bringup/config/blue.yaml b/blue_bringup/config/blue.yaml index b8335012..87df36ee 100644 --- a/blue_bringup/config/blue.yaml +++ b/blue_bringup/config/blue.yaml @@ -30,24 +30,15 @@ ismc: aruco_marker_localizer: ros__parameters: camera_matrix: - [1078.17559, 0.0, 1010.57086, 0.0, 1076.46176, 463.06243, 0.0, 0.0, 1.0] + [1078.17559, 0.0, 1010.57086, + 0.0, 1076.46176, 463.06243, + 0.0, 0.0, 1.0] distortion_coefficients: [0.019645, 0.007271, -0.004324, -0.001628, 0.000000] projection_matrix: - [ - 1108.25366, - 0.0, - 1003.75555, - 0.0, - 0.0, - 1108.39001, - 456.92861, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - ] + [1108.25366, 0.0, 1003.75555, 0.0, + 0.0, 1108.39001, 456.92861, 0.0, + 0.0, 0.0, 1.0, 0.0] camera: ros__parameters: @@ -58,11 +49,11 @@ qualisys_mocap: ip: "192.168.0.0" port: 22223 version: "1.22" - body: "angler" + body: "bluerov" qualisys_localizer: ros__parameters: - body: "angler" # This should be the same as the body parameter setting for the qualisys_mocap node + body: "bluerov" # This should be the same as the body parameter setting for the qualisys_mocap node mavros: ros__parameters: diff --git a/blue_bringup/test/test_pep257.py b/blue_bringup/test/test_pep257.py deleted file mode 100644 index 4eddb46e..00000000 --- a/blue_bringup/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import pytest -from ament_pep257.main import main - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=[".", "test"]) - assert rc == 0, "Found code style errors / warnings" diff --git a/blue_control/src/controller.cpp b/blue_control/src/controller.cpp index 4c6760d6..4328b922 100644 --- a/blue_control/src/controller.cpp +++ b/blue_control/src/controller.cpp @@ -153,9 +153,6 @@ Controller::Controller(const std::string & node_name) // Convert the control loop frequency to seconds dt_ = 1 / this->get_parameter("control_loop_freq").as_double(); - // Run the controller at a rate of 200 Hz - // ArduSub only runs at a rate of 100 Hz, but we want to make sure to run the controller - // at a faster rate than the autopilot control_loop_timer_ = this->create_wall_timer(std::chrono::duration(dt_), [this]() -> void { if (armed_) { diff --git a/blue_localization/blue_localization/localizer.py b/blue_localization/blue_localization/localizer.py index 9ece1a5d..807821fe 100644 --- a/blue_localization/blue_localization/localizer.py +++ b/blue_localization/blue_localization/localizer.py @@ -51,7 +51,7 @@ def __init__(self, node_name: str) -> None: self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - # SLAM poses are sent to the ArduPilot EKF + # Poses are sent to the ArduPilot EKF self.localization_pub = self.create_publisher( PoseStamped, "/mavros/vision_pose/pose", 1 )