Merge pull request #11 from IOES-Lab/usbl-plugins

[GSOC-67] Migration of USBL plugins

.pre-commit-config.yaml

@@ -80,24 +80,25 @@ repos:
       - id: clang-format
         args: ["-fallback-style=none", "-i"]
 
-  - repo: local
-    hooks:
-      - id: ament_cppcheck
-        name: ament_cppcheck
-        description: Static code analysis of C/C++ files.
-        entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
-        language: system
-        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+  # - repo: local
+  #   hooks:
+  #     - id: ament_cppcheck
+  #       name: ament_cppcheck
+  #       description: Static code analysis of C/C++ files.
+  #       entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
+  #       language: system
+  #       files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
 
-  - repo: local
-    hooks:
-      - id: ament_cpplint
-        name: ament_cpplint
-        description: Static code analysis of C/C++ files.
-        entry: ament_cpplint
-        language: system
-        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
-        args: ["--linelength=100", "--filter=-whitespace/newline"]
+  # - repo: https://github.com/cpplint/cpplint
+  #   rev: 1.6.1
+  #   hooks:
+  #     - id: cpplint
+  #       name: cpplint
+  #       description: Static code analysis of C/C++ files
+  #       language: python
+  #       files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+  #       entry: cpplint
+  #       args: ["--linelength=100", "--filter=-whitespace/newline"]
 
   # Cmake hooks (can be installed with pip3 install ament-lint-cmake-py)
   - repo: local

dave_interfaces/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.8)
+project(dave_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/UsblCommand.msg"
+  "msg/UsblResponse.msg"
+  "msg/Location.msg"
+)
+
+# Install message package.xml
+install(FILES
+  package.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+# Install CMake package configuration
+ament_export_include_directories(include)
+
+ament_package()

dave_interfaces/msg/Location.msg

@@ -0,0 +1,4 @@
+int32 transponder_id
+float64 x
+float64 y
+float64 z

dave_interfaces/msg/UsblCommand.msg

@@ -0,0 +1,3 @@
+int32 transponder_id
+int32 command_id
+string data

dave_interfaces/msg/UsblResponse.msg

@@ -0,0 +1,3 @@
+int32 transceiver_id
+int32 response_id
+string data

dave_interfaces/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
+  <name>dave_interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="helenamoyen@usp.br">lena</maintainer>
+  <license>TODO: License declaration</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

examples/dave_demos/launch/dave_world.launch.py

@@ -12,14 +12,19 @@ def launch_setup(context, *args, **kwargs):
     pkg_ros_gz_sim = get_package_share_directory("ros_gz_sim")
 
     world_name = LaunchConfiguration("world_name").perform(context)
+    verbose_flag = LaunchConfiguration("verbose").perform(context)
     world_file_name = f"{world_name}.world"
 
     world_path = os.path.join(pkg_dave_worlds, "worlds", world_file_name)
 
     # Gazebo simulation launch
+    gz_args = f"-r {world_path}"
+    if verbose_flag.lower() == "true":
+        gz_args += " --verbose"
+
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, "launch", "gz_sim.launch.py")),
-        launch_arguments={"gz_args": f"-r {world_path}"}.items(),
+        launch_arguments={"gz_args": gz_args}.items(),
     )
 
     return [gz_sim]
@@ -30,9 +35,14 @@ def generate_launch_description():
         [
             DeclareLaunchArgument(
                 "world_name",
-                default_value="dave_bimanual_example",  # Default world file name without extension
+                default_value="dave_bimanual_example",
                 description="Name of the world file",
             ),
+            DeclareLaunchArgument(
+                "verbose",
+                default_value="false",
+                description="Enable verbose mode for Gazebo simulation",
+            ),
             OpaqueFunction(function=launch_setup),
         ]
     )

gazebo/dave_gz_sensor_plugins/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.8)
+
+# Define the project name
+project(dave_gz_sensor_plugins)
+
+# Find required packages
+find_package(ament_cmake REQUIRED)
+find_package(gz-cmake3 REQUIRED)
+find_package(gz-plugin2 REQUIRED COMPONENTS register)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(gz-common5 REQUIRED COMPONENTS profiler)
+find_package(gz-sim8 REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(dave_interfaces REQUIRED)
+
+# Set version variables
+set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
+set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})
+set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
+
+message(STATUS "Compiling against Gazebo Harmonic")
+
+add_library(UsblTransceiver SHARED src/UsblTransceiver.cc)
+add_library(UsblTransponder SHARED src/UsblTransponder.cc)
+
+target_include_directories(UsblTransceiver PRIVATE include)
+target_include_directories(UsblTransponder PRIVATE include)
+
+target_link_libraries(UsblTransceiver
+  gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
+
+target_link_libraries(UsblTransponder
+gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER})
+
+
+# Specify dependencies for FullSystem using ament_target_dependencies
+ament_target_dependencies(UsblTransceiver
+  dave_interfaces
+  rclcpp
+  geometry_msgs
+  std_msgs
+)
+
+ament_target_dependencies(UsblTransponder
+  dave_interfaces
+  rclcpp
+  std_msgs
+)
+
+# Install targets
+install(TARGETS UsblTransceiver UsblTransponder
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install headers
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+# Environment hooks
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")
+
+# Testing setup
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Configure ament
+ament_package()

gazebo/dave_gz_sensor_plugins/hooks/dave_gz_sensor_plugins.dsv.in

@@ -0,0 +1 @@
+prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib/@PROJECT_NAME@/

gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransceiver.hh

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+
+#ifndef DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_
+#define DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_
+
+#include <gz/msgs/vector3d.pb.h>
+#include <math.h>
+#include <memory>
+#include <string>
+
+#include <gz/sim/System.hh>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
+#include "dave_interfaces/msg/location.hpp"
+#include "dave_interfaces/msg/usbl_command.hpp"
+#include "dave_interfaces/msg/usbl_response.hpp"
+
+namespace dave_gz_sensor_plugins
+
+{
+class UsblTransceiver : public gz::sim::System,
+                        public gz::sim::ISystemConfigure,
+                        public gz::sim::ISystemPostUpdate
+{
+public:
+  UsblTransceiver();
+  ~UsblTransceiver() override = default;
+
+  void Configure(
+    const gz::sim::Entity & entity, const std::shared_ptr<const sdf::Element> & sdf,
+    gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override;
+
+  void PostUpdate(
+    const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override;
+
+  void receiveGazeboCallback(
+    const std::string & transponder, const gz::msgs::Vector3d & transponder_position);
+  void temperatureRosCallback(const std_msgs::msg::Float64::SharedPtr msg);
+  void interrogationModeRosCallback(const std_msgs::msg::String::SharedPtr msg);
+  void commandingResponseCallback(const dave_interfaces::msg::UsblResponse msg);
+  void channelSwitchCallback(const std_msgs::msg::String::SharedPtr msg);
+  void commandingResponseTestCallback(const std_msgs::msg::String::SharedPtr msg);
+  void sendCommand(int command_id, std::string & transponder_id);
+  void sendPing();
+  void calculateRelativePose(
+    gz::math::Vector3<double> position, double & bearing, double & range, double & elevation);
+  void publishPosition(int & transponder_id, double & bearing, double & range, double & elevation);
+
+private:
+  std::shared_ptr<rclcpp::Node> ros_node_;
+
+  struct PrivateData;
+  std::unique_ptr<PrivateData> dataPtr;
+};
+}  // namespace dave_gz_sensor_plugins
+
+#endif  // DAVE_GZ_SENSOR_PLUGINS__USBLTRANSCEIVER_HH_

gazebo/dave_gz_sensor_plugins/include/dave_gz_sensor_plugins/UsblTransponder.hh

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+ */
+
+#ifndef DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_
+#define DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_
+
+#include <memory>
+#include <string>
+
+#include <gz/sim/System.hh>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/string.hpp>
+#include "dave_interfaces/msg/usbl_command.hpp"
+#include "dave_interfaces/msg/usbl_response.hpp"
+
+namespace dave_gz_sensor_plugins
+
+{
+class UsblTransponder : public gz::sim::System,
+                        public gz::sim::ISystemConfigure,
+                        public gz::sim::ISystemPostUpdate
+{
+public:
+  UsblTransponder();
+  ~UsblTransponder() override = default;
+
+  void Configure(
+    const gz::sim::Entity & entity, const std::shared_ptr<const sdf::Element> & sdf,
+    gz::sim::EntityComponentManager & ecm, gz::sim::EventManager & eventMgr) override;
+
+  void PostUpdate(
+    const gz::sim::UpdateInfo & info, const gz::sim::EntityComponentManager & ecm) override;
+
+  void sendLocation();
+  void iisRosCallback(const std_msgs::msg::String::SharedPtr msg);
+  void temperatureRosCallback(const std_msgs::msg::Float64::SharedPtr msg);
+  void cisRosCallback(const std_msgs::msg::String::SharedPtr msg);
+  void commandRosCallback(const dave_interfaces::msg::UsblCommand msg);
+  void sendPing();
+  void interrogationModeCallback(const std_msgs::msg::String::SharedPtr msg);
+
+private:
+  std::shared_ptr<rclcpp::Node> ros_node_;
+
+  struct PrivateData;
+  std::unique_ptr<PrivateData> dataPtr;
+};
+}  // namespace dave_gz_sensor_plugins
+
+#endif  // DAVE_GZ_SENSOR_PLUGINS__USBLTRANSPONDER_HH_

gazebo/dave_gz_sensor_plugins/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?><package format="3">
+  <name>dave_gz_sensor_plugins</name>
+  <version>0.0.0</version>
+  <description>DAVE sensor plugins</description>
+  <maintainer email="helenamoyen@gmail.com">Helena Moyen</maintainer>
+  <license>Apache 2.0</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <build_depend>dave_interfaces</build_depend>
+  <exec_depend>dave_interfaces</exec_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>