diff --git a/.gitignore b/.gitignore index d48fb5bd..3ba8fc77 100644 --- a/.gitignore +++ b/.gitignore @@ -76,3 +76,8 @@ CATKIN_IGNORE # ignore mac DS files *.DS_Store + +# pytest cache +.pytest_cache/ +__pycache__/ +*.pyc diff --git a/.travis.yml b/.travis.yml index 113ea1a1..accb47af 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,142 +1,61 @@ -# Copyright (c) 2016, Felix Duvallet -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# -# * Neither the name of this package nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.# -# -# FROM: https://github.com/felixduvallet/ros-travis-integration +# Travis CI configuration for ROS 2 (Humble) on Ubuntu 22.04 # -# Generic .travis.yml file for running continuous integration on Travis-CI with -# any ROS package. -# -# This installs ROS on a clean Travis-CI virtual machine, creates a ROS -# workspace, resolves all listed dependencies, and sets environment variables -# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are -# no compilation errors), and runs all the tests. If any of the compilation/test -# phases fail, the build is marked as a failure. -# -# We handle two types of package dependencies: -# - packages (ros and otherwise) available through apt-get. These are installed -# using rosdep, based on the information in the ROS package.xml. -# - dependencies that must be checked out from source. These are handled by -# 'wstool', and should be listed in a file named dependencies.rosinstall. -# -# There are two variables you may want to change: -# - ROS_DISTRO (default is indigo). Note that packages must be available for -# ubuntu 14.04 trusty. -# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo -# root). This should list all necessary repositories in wstool format (see -# the ros wiki). If the file does not exists then nothing happens. -# -# See the README.md for more information. -# -# Author: Felix Duvallet - -# NOTE: The build lifecycle on Travis.ci is something like this: -# before_install -# install -# before_script -# script -# after_success or after_failure -# after_script -# OPTIONAL before_deploy -# OPTIONAL deploy -# OPTIONAL after_deploy - -################################################################################ +# Builds the sailing_robot and xsens_driver packages with colcon, then runs +# the pure-Python unit tests (no ROS master required) via pytest. -# Use ubuntu trusty (14.04) with sudo privileges. -dist: trusty -sudo: required -language: - - generic +dist: jammy +language: generic cache: - apt -# Configuration variables. All variables are global now, but this can be used to -# trigger a build matrix for different ROS distributions if desired. env: global: - - ROS_DISTRO=indigo - - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] + - ROS_DISTRO=humble - CI_SOURCE_PATH=$(pwd) - - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall - - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options - - ROS_PARALLEL_JOBS='-j8 -l6' -################################################################################ - -# Install system dependencies, namely a very barebones ROS setup. before_install: - - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + # Add the ROS 2 apt repository - sudo apt-get update -qq - - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-dynamic-reconfigure python-dev python-numpy libgeos-dev - - source /opt/ros/$ROS_DISTRO/setup.bash - # Prepare rosdep to install dependencies. - - sudo rosdep init - - rosdep update + - sudo apt-get install -y curl gnupg2 lsb-release + - sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg + - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \ + | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + - sudo apt-get update -qq + - sudo apt-get install -y \ + python3-colcon-common-extensions \ + python3-rosdep \ + python3-pip \ + ros-$ROS_DISTRO-ros-base \ + ros-$ROS_DISTRO-ament-cmake \ + ros-$ROS_DISTRO-ament-cmake-python \ + ros-$ROS_DISTRO-rosidl-default-generators \ + ros-$ROS_DISTRO-rosidl-default-runtime \ + ros-$ROS_DISTRO-std-msgs \ + ros-$ROS_DISTRO-sensor-msgs \ + ros-$ROS_DISTRO-geometry-msgs \ + ros-$ROS_DISTRO-visualization-msgs \ + ros-$ROS_DISTRO-diagnostic-msgs \ + ros-$ROS_DISTRO-tf2-ros \ + libgeos-dev + - pip3 install --user LatLon23 shapely pynmea2 scipy numpy pyproj -# Create a catkin workspace with the package under integration. install: - - mkdir -p ~/catkin_ws/src - - cd ~/catkin_ws/src - - catkin_init_workspace - # Create the devel/setup.bash (run catkin_make with an empty workspace) and - # source it to set the path variables. - - cd ~/catkin_ws - # Travis adds a non-system Python. Put /usr/bin/python before it on PATH. - - export PATH=$(echo "$PATH" | sed -r "s|/opt/python/2.7.[0-9]+/bin|/usr/bin:&|" ) - - echo $PATH - - which python - - catkin_make - - source devel/setup.bash - # Add the package under integration to the workspace using a symlink. - - cd ~/catkin_ws/src - - ln -s $CI_SOURCE_PATH . - - pip install --user LatLon shapely pynmea2 - -# Install all dependencies, using wstool and rosdep. -# wstool looks for a ROSINSTALL_FILE defined in the environment variables. -before_script: - # source dependencies: install using wstool. - - cd ~/catkin_ws/src - - wstool init - - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi - - wstool up - # package depdencies: install using rosdep. - - cd ~/catkin_ws - - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO + - sudo rosdep init || true + - rosdep update + - mkdir -p ~/ros2_ws/src + - ln -s $CI_SOURCE_PATH/src/sailing_robot ~/ros2_ws/src/sailing_robot + - ln -s $CI_SOURCE_PATH/src/xsens_driver ~/ros2_ws/src/xsens_driver + - source /opt/ros/$ROS_DISTRO/setup.bash + - cd ~/ros2_ws + - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO || true -# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to -# catkin_make. script: - source /opt/ros/$ROS_DISTRO/setup.bash - - cd ~/catkin_ws - - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) - - source ~/catkin_ws/devel/setup.bash - - catkin_make run_tests - - catkin_test_results - # Testing: Use both run_tests (to see the output) and test (to error out). + - cd ~/ros2_ws + # Build + - colcon build --symlink-install --packages-select sailing_robot xsens_driver + # Run pure-Python unit tests (no ROS daemon needed) + - source install/setup.bash + - cd $CI_SOURCE_PATH + - python3 -m pytest src/sailing_robot/tests/ -v diff --git a/src/sailing_robot/CMakeLists.txt b/src/sailing_robot/CMakeLists.txt index deeb19e1..ed60d89a 100644 --- a/src/sailing_robot/CMakeLists.txt +++ b/src/sailing_robot/CMakeLists.txt @@ -1,183 +1,97 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(sailing_robot) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - sensor_msgs - message_generation - dynamic_reconfigure -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() -################################################ -## Declare ROS messages, services and actions ## -################################################ +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -# Generate messages in the 'msg' folder -add_message_files( - FILES - Velocity.msg - gpswtime.msg - BatteryState.msg +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# Generate ROS messages +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Velocity.msg" + "msg/gpswtime.msg" + "msg/BatteryState.msg" + DEPENDENCIES std_msgs sensor_msgs ) -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here - generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs - ) - - generate_dynamic_reconfigure_options( - cfg/TackVoting.cfg - #... - ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES sailing_robot - CATKIN_DEPENDS rospy std_msgs sensor_msgs message_runtime -# DEPENDS system_lib +# Install the Python package (sailing_robot library) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/sailing_robot ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} +# Install node scripts +install(PROGRAMS + scripts/actuator_demand_rudder + scripts/actuator_demand_sail + scripts/actuator_driver_rudder + scripts/actuator_driver_sail + scripts/debugging_2D_plot + scripts/debugging_2D_plot_matplot + scripts/debugging_blink_on_sailing_state + scripts/debugging_dashboard + scripts/debugging_dump_params + scripts/debugging_gps_log + scripts/dummy_apparent_wind_direction + scripts/dummy_heading + scripts/dummy_position + scripts/force_jibe_tack + scripts/helming + scripts/sensor_camera_detect + scripts/sensor_driver_battery + scripts/sensor_driver_gps + scripts/sensor_driver_imu + scripts/sensor_driver_imu_fusion + scripts/sensor_driver_imu_without_cali + scripts/sensor_driver_multiplexer + scripts/sensor_driver_wind_direction + scripts/sensor_processed_wind_direction + scripts/sensor_service_imu + scripts/simulation_gps_fix + scripts/simulation_heading + scripts/simulation_position + scripts/simulation_velocity + scripts/simulation_wind_apparent + scripts/tack + scripts/tasks + scripts/wave_period + scripts/wave_position + DESTINATION lib/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(sailing_robot -# src/${PROJECT_NAME}/sailing_robot.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(sailing_robot ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(sailing_robot_node src/sailing_robot_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(sailing_robot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(sailing_robot_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS sailing_robot sailing_robot_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_sailing_robot.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +# Install parameter files +install(DIRECTORY + launch/parameters + DESTINATION share/${PROJECT_NAME}/launch +) -if (CATKIN_ENABLE_TESTING) - # rostests - find_package(rostest REQUIRED) - add_rostest(rostests/test-1.test) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() - # Python nosetests - catkin_add_nosetests(tests) + find_package(ament_cmake_pytest REQUIRED) + ament_add_pytest_test(sailing_robot_tests tests) endif() + +ament_package() diff --git a/src/sailing_robot/launch/simulator.launch.py b/src/sailing_robot/launch/simulator.launch.py new file mode 100644 index 00000000..29dc9c54 --- /dev/null +++ b/src/sailing_robot/launch/simulator.launch.py @@ -0,0 +1,90 @@ +""" +ROS 2 launch file for the sailing robot simulator. + +Equivalent to the ROS 1 simulator.launch file. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +import yaml + + +def _params(filename): + """Return the full path of a parameter file shipped with this package.""" + return os.path.join( + get_package_share_directory('sailing_robot'), + 'launch', 'parameters', filename) + + +def generate_launch_description(): + params_files = [ + _params('default.yaml'), + _params('calibration_blackpython.yaml'), + _params('sailsettings_blackpython.yaml'), + _params('servos_blackpython.yaml'), + _params('sailingClub_waypoints.yaml'), + _params('simulator.yaml'), + ] + + shared_params = [{'log_name': 'simulator_test'}] + params_files + + return LaunchDescription([ + Node( + package='sailing_robot', + executable='tasks', + name='tasks', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='helming', + name='helming', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='simulation_position', + name='simulation_position', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='simulation_velocity', + name='simulation_velocity', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='simulation_wind_apparent', + name='simulation_wind_apparent', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='simulation_heading', + name='simulation_heading', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='sensor_processed_wind_direction', + name='sensor_processed_wind_direction', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='simulation_gps_fix', + name='simulation_gps_fix', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='debugging_dashboard', + name='debugging_dashboard', + parameters=shared_params, + ), + ]) diff --git a/src/sailing_robot/launch/standby.launch.py b/src/sailing_robot/launch/standby.launch.py new file mode 100644 index 00000000..440ab566 --- /dev/null +++ b/src/sailing_robot/launch/standby.launch.py @@ -0,0 +1,52 @@ +""" +ROS 2 launch file for standby mode (hardware drivers only). + +Equivalent to the ROS 1 standby.launch file. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def _params(filename): + """Return the full path of a parameter file shipped with this package.""" + return os.path.join( + get_package_share_directory('sailing_robot'), + 'launch', 'parameters', filename) + + +def generate_launch_description(): + params_files = [ + _params('default.yaml'), + _params('calibration_blackpython.yaml'), + _params('sailsettings_blackpython_rigA.yaml'), + _params('servos_blackpython.yaml'), + ] + + shared_params = [{'do_post': True}] + params_files + + return LaunchDescription([ + Node( + package='sailing_robot', + executable='sensor_driver_gps', + name='sensor_driver_gps', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='actuator_driver_rudder', + name='actuator_driver_rudder', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='actuator_driver_sail', + name='actuator_driver_sail', + parameters=shared_params, + respawn=True, + ), + ]) diff --git a/src/sailing_robot/launch/test_calibration.launch.py b/src/sailing_robot/launch/test_calibration.launch.py new file mode 100644 index 00000000..00c1a8a3 --- /dev/null +++ b/src/sailing_robot/launch/test_calibration.launch.py @@ -0,0 +1,96 @@ +""" +ROS 2 launch file for calibration testing (all hardware sensors). + +Equivalent to the ROS 1 test-calibration.launch file. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def _params(filename): + """Return the full path of a parameter file shipped with this package.""" + return os.path.join( + get_package_share_directory('sailing_robot'), + 'launch', 'parameters', filename) + + +def generate_launch_description(): + params_files = [ + _params('default.yaml'), + _params('calibration_blackpython.yaml'), + _params('sailsettings_blackpython_rigA.yaml'), + _params('servos_blackpython.yaml'), + ] + + shared_params = [{'do_post': True}] + params_files + + xsens_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory('xsens_driver'), + 'launch', 'xsens_driver.launch.py') + ) + ) + + return LaunchDescription([ + xsens_launch, + Node( + package='sailing_robot', + executable='sensor_service_imu', + name='sensor_service_imu', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='sensor_driver_gps', + name='sensor_driver_gps', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='sensor_driver_wind_direction', + name='sensor_driver_wind_direction', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='sensor_driver_multiplexer', + name='sensor_driver_multiplexer', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='actuator_driver_rudder', + name='actuator_driver_rudder', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='actuator_driver_sail', + name='actuator_driver_sail', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='sensor_driver_battery', + name='sensor_driver_battery', + parameters=shared_params, + ), + Node( + package='sailing_robot', + executable='debugging_dashboard', + name='debugging_dashboard', + parameters=shared_params, + ), + ]) diff --git a/src/sailing_robot/launch/test_gps.launch.py b/src/sailing_robot/launch/test_gps.launch.py new file mode 100644 index 00000000..38cc4492 --- /dev/null +++ b/src/sailing_robot/launch/test_gps.launch.py @@ -0,0 +1,52 @@ +""" +ROS 2 launch file for testing the GPS sensor. + +Equivalent to the ROS 1 test-gps.launch file. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def _params(filename): + """Return the full path of a parameter file shipped with this package.""" + return os.path.join( + get_package_share_directory('sailing_robot'), + 'launch', 'parameters', filename) + + +def generate_launch_description(): + params_files = [ + _params('default.yaml'), + _params('calibration_blackpython.yaml'), + _params('sailsettings_blackpython.yaml'), + _params('servos_blackpython.yaml'), + _params('sailingClub_waypoints.yaml'), + ] + + shared_params = [{'log_name': 'sailingclub_tests'}] + params_files + + return LaunchDescription([ + Node( + package='sailing_robot', + executable='sensor_driver_gps', + name='sensor_driver_gps', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='debugging_gps_log', + name='debugging_gps_log', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='debugging_dashboard', + name='debugging_dashboard', + parameters=shared_params, + ), + ]) diff --git a/src/sailing_robot/launch/test_imu.launch.py b/src/sailing_robot/launch/test_imu.launch.py new file mode 100644 index 00000000..e8734fda --- /dev/null +++ b/src/sailing_robot/launch/test_imu.launch.py @@ -0,0 +1,39 @@ +""" +ROS 2 launch file for testing the IMU sensor. + +Equivalent to the ROS 1 test-imu.launch file. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def _params(filename): + """Return the full path of a parameter file shipped with this package.""" + return os.path.join( + get_package_share_directory('sailing_robot'), + 'launch', 'parameters', filename) + + +def generate_launch_description(): + params_files = [ + _params('default.yaml'), + _params('calibration_blackpython.yaml'), + _params('sailsettings_blackpython.yaml'), + _params('servos_blackpython.yaml'), + _params('sailingClub_waypoints.yaml'), + ] + + shared_params = [{'log_name': 'sailingclub_tests'}] + params_files + + return LaunchDescription([ + Node( + package='sailing_robot', + executable='sensor_driver_imu', + name='sensor_driver_imu', + parameters=shared_params, + respawn=True, + ), + ]) diff --git a/src/sailing_robot/launch/test_wind_direction.launch.py b/src/sailing_robot/launch/test_wind_direction.launch.py new file mode 100644 index 00000000..1f6b478f --- /dev/null +++ b/src/sailing_robot/launch/test_wind_direction.launch.py @@ -0,0 +1,51 @@ +""" +ROS 2 launch file for testing the wind direction sensor and compass calibration. + +Equivalent to the ROS 1 test-wind-direction.launch file. +""" + +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def _params(filename): + """Return the full path of a parameter file shipped with this package.""" + return os.path.join( + get_package_share_directory('sailing_robot'), + 'launch', 'parameters', filename) + + +def generate_launch_description(): + params_files = [ + _params('default.yaml'), + _params('calibration_blackpython.yaml'), + _params('sailsettings_blackpython_rigA.yaml'), + _params('servos_blackpython.yaml'), + ] + + shared_params = [{'do_post': True}] + params_files + + return LaunchDescription([ + Node( + package='sailing_robot', + executable='sensor_service_imu', + name='sensor_service_imu', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='sensor_driver_wind_direction', + name='sensor_driver_wind_direction', + parameters=shared_params, + respawn=True, + ), + Node( + package='sailing_robot', + executable='debugging_dashboard', + name='debugging_dashboard', + parameters=shared_params, + ), + ]) diff --git a/src/sailing_robot/package.xml b/src/sailing_robot/package.xml index 6a620402..4e30860a 100644 --- a/src/sailing_robot/package.xml +++ b/src/sailing_robot/package.xml @@ -1,62 +1,35 @@ - + + sailing_robot 0.0.0 The sailing_robot package - - - - sophia - - - - - MIT + ament_cmake + ament_cmake_python - - - - - - - - - - - - - - - - - - - - - - - - catkin - python-catkin-pkg - rospy + rosidl_default_generators std_msgs sensor_msgs - message_generation - robot_localization - message_runtime - rospy - std_msgs - sensor_msgs - robot_localization - rostest + rclpy + std_msgs + sensor_msgs + geometry_msgs + visualization_msgs + rosidl_default_runtime + tf2_ros + tf_transformations - - - + rosidl_interface_packages + ament_lint_auto + ament_lint_common + pytest + + + ament_cmake diff --git a/src/sailing_robot/resource/sailing_robot b/src/sailing_robot/resource/sailing_robot new file mode 100644 index 00000000..e69de29b diff --git a/src/sailing_robot/rostests/test_1.py b/src/sailing_robot/rostests/test_1.py index 314901ff..fbc974eb 100755 --- a/src/sailing_robot/rostests/test_1.py +++ b/src/sailing_robot/rostests/test_1.py @@ -1,70 +1,73 @@ -#!/usr/bin/env python -# -# based on:http://docs.ros.org/diamondback/api/rospy_tutorials/html/test__on__shutdown_8py_source.html -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Revision $Id: test_peer_subscribe_notify.py 3803 2009-02-11 02:04:39Z rob_wheeler $ - -## Integration test for peer_subscribe_notify - -PKG = 'rospy_tutorials' -NAME = 'peer_subscribe_notify_test' +#!/usr/bin/env python3 +""" +Integration test for the tack node (ROS 2 version). +This test publishes a sailing_state message and checks that tack_rudder is +published in response. It uses rclpy directly (no rostest/roslaunch needed) +and is run by pytest. + +NOTE: This test requires a running ROS 2 daemon and the ``tack`` executable +to be available on PATH (i.e. the package must be installed/sourced first). +If rclpy is not available the test is skipped so that the plain unit-test +suite still passes in environments without ROS 2 installed. +""" + +import subprocess import sys import time import unittest -from Queue import Queue - -import rospy -import rostest -import roslib.scriptutil as scriptutil -from std_msgs.msg import String, Float32 - -def subscribe_queue(topic, msg_type): - q = Queue() - rospy.Subscriber(topic, msg_type, q.put) - return q - -class TestOnShutdown(unittest.TestCase): - def test_notify(self): - q = subscribe_queue("/tack_rudder", Float32) - rospy.init_node(NAME, anonymous=True) - p = rospy.Publisher("/sailing_state", String, queue_size=10) - time.sleep(0.2) # Icky fudge factor to give the tack node time to be ready - p.publish('switch_to_stbd_tack') - msg = q.get(timeout=2) - self.assertEqual(msg.data, 90) + +try: + import rclpy + from rclpy.node import Node + from std_msgs.msg import String, Float32 + ROS2_AVAILABLE = True +except ImportError: + ROS2_AVAILABLE = False + + +@unittest.skipUnless(ROS2_AVAILABLE, 'rclpy not available') +class TestTackNode(unittest.TestCase): + """Test that the tack node responds to sailing_state messages.""" + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = Node('test_tack_node') + cls.received = [] + + cls.sub = cls.node.create_subscription( + Float32, '/tack_rudder', + lambda msg: cls.received.append(msg.data), + 10) + + cls.pub = cls.node.create_publisher(String, '/sailing_state', 10) + + # Give the tack node (started externally) time to come up + time.sleep(0.2) + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_tack_rudder_published(self): + """Publishing switch_to_stbd_tack should produce a tack_rudder value.""" + msg = String() + msg.data = 'switch_to_stbd_tack' + deadline = time.time() + 2.0 + while time.time() < deadline: + self.pub.publish(msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + if self.received: + break + + self.assertTrue( + len(self.received) > 0, + 'No tack_rudder message received within 2 seconds') + self.assertEqual(self.received[0], 90.0) + if __name__ == '__main__': - rostest.rosrun(PKG, NAME, TestOnShutdown, sys.argv) + unittest.main() + diff --git a/src/sailing_robot/scripts/actuator_demand_rudder b/src/sailing_robot/scripts/actuator_demand_rudder index 94656895..792573f1 100755 --- a/src/sailing_robot/scripts/actuator_demand_rudder +++ b/src/sailing_robot/scripts/actuator_demand_rudder @@ -1,53 +1,68 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32, Int16, String from sailing_robot.pid_data import PID_Data import sailing_robot.pid_control as _PID from sailing_robot.navigation import angle_subtract -data = PID_Data() -rudder = rospy.get_param('rudder') +class ActuatorDemandRudder(Node): + def __init__(self): + super().__init__('actuator_demand_rudder') + self.data = PID_Data() -controller = _PID.PID(rudder['control']['Kp'], rudder['control']['Ki'], rudder['control']['Kd'],rudder['maxAngle'], -rudder['maxAngle']) + self.declare_parameter('rudder.control.Kp', 0.5) + self.declare_parameter('rudder.control.Ki', 0.0) + self.declare_parameter('rudder.control.Kd', 0.0) + self.declare_parameter('rudder.maxAngle', 40.0) + self.declare_parameter('config.rate', 10) + kp = self.get_parameter('rudder.control.Kp').value + ki = self.get_parameter('rudder.control.Ki').value + kd = self.get_parameter('rudder.control.Kd').value + self.max_angle = self.get_parameter('rudder.maxAngle').value + rate_hz = self.get_parameter('config.rate').value -def node_publisher(): - """ - Publish rudder servo angle data (Int16) to Arduino node. - Higher level tack angle was used when in TACK manoeuvre. - PID controller was used in other conditions. - :rtype: object - """ - pub = rospy.Publisher('rudder_control', Int16, queue_size=10) # Use UInt 16 here to minimize the memory use - rospy.init_node('actuator_demand_rudder', anonymous=True) - rate = rospy.Rate(10) + self.controller = _PID.PID(kp, ki, kd, self.max_angle, -self.max_angle) - while not rospy.is_shutdown(): - rospy.loginfo("Sailing state: %r", data.sailing_state) - if data.sailing_state == 'normal': - rawangle = -controller.update_PID(angle_subtract( - data.heading, data.goal_heading)) - angle = _PID.saturation(rawangle,-rudder['maxAngle'], rudder['maxAngle']) - rospy.loginfo("Angle: %r", angle) - else: - rawangle = data.tack_rudder - angle = _PID.saturation(rawangle,-rudder['maxAngle'], rudder['maxAngle']) + self.pub = self.create_publisher(Int16, 'rudder_control', 10) + self.create_subscription(Float32, 'goal_heading', self.data.update_goal_heading, 10) + self.create_subscription(Float32, 'heading', self.data.update_heading, 10) + self.create_subscription(String, 'sailing_state', self.data.update_sailing_state, 10) + self.create_subscription(Float32, 'tack_rudder', self.data.update_tack_rudder, 10) + + self.timer = self.create_timer(1.0 / rate_hz, self.publish_rudder) - pub.publish(int(angle)) + def publish_rudder(self): + self.get_logger().debug('Sailing state: %s' % self.data.sailing_state) + if self.data.sailing_state == 'normal': + rawangle = -self.controller.update_PID(angle_subtract( + self.data.heading, self.data.goal_heading)) + angle = _PID.saturation(rawangle, -self.max_angle, self.max_angle) + self.get_logger().debug('Angle: %s' % angle) + else: + rawangle = self.data.tack_rudder + angle = _PID.saturation(rawangle, -self.max_angle, self.max_angle) - rate.sleep() + msg = Int16() + msg.data = int(angle) + self.pub.publish(msg) -print(data.goal_heading) -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = ActuatorDemandRudder() try: - rospy.Subscriber('goal_heading', Float32, data.update_goal_heading) - rospy.Subscriber('heading', Float32, data.update_heading) - rospy.Subscriber('sailing_state', String, data.update_sailing_state) - rospy.Subscriber('tack_rudder', Float32, data.update_tack_rudder) - node_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/actuator_demand_sail b/src/sailing_robot/scripts/actuator_demand_sail index 46aec0df..f8febe6e 100755 --- a/src/sailing_robot/scripts/actuator_demand_sail +++ b/src/sailing_robot/scripts/actuator_demand_sail @@ -1,31 +1,56 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Float64, Float32, String from sailing_robot.sail_table import SailTable, SailData import sailing_robot.pid_control as _PID -sail_table_dict = rospy.get_param('sailsettings/table') -sheet_out_to_jibe = rospy.get_param('sailsettings/sheet_out_to_jibe', False) -sail_table = SailTable(sail_table_dict) -sail_data = SailData(sail_table) -def node_publisher(): - pub = rospy.Publisher('sailsheet_normalized', Float32, queue_size=10) - rospy.init_node('actuator_demand_sail', anonymous=True) +class ActuatorDemandSail(Node): + def __init__(self): + super().__init__('actuator_demand_sail') - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - sheet_normalized = sail_data.calculate_sheet_setting() - pub.publish(sheet_normalized) - rate.sleep() + self.declare_parameter('sailsettings.table', '{}') + self.declare_parameter('sailsettings.sheet_out_to_jibe', False) + self.declare_parameter('config.rate', 10) + sail_table_dict = self.get_parameter('sailsettings.table').value + sheet_out_to_jibe = self.get_parameter('sailsettings.sheet_out_to_jibe').value + rate_hz = self.get_parameter('config.rate').value -if __name__ == '__main__': + if isinstance(sail_table_dict, str): + import yaml + sail_table_dict = yaml.safe_load(sail_table_dict) or {} + + sail_table = SailTable(sail_table_dict) + self.sail_data = SailData(sail_table) + + self.pub = self.create_publisher(Float32, 'sailsheet_normalized', 10) + self.create_subscription(Float64, 'wind_direction_apparent', self.sail_data.update_wind, 10) + self.create_subscription(String, 'sailing_state', self.sail_data.update_sailing_state, 10) + + self.timer = self.create_timer(1.0 / rate_hz, self.publish_sail) + + def publish_sail(self): + sheet_normalized = self.sail_data.calculate_sheet_setting() + msg = Float32() + msg.data = float(sheet_normalized) + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = ActuatorDemandSail() try: - rospy.Subscriber('wind_direction_apparent', Float64, sail_data.update_wind) - rospy.Subscriber('sailing_state', String, sail_data.update_sailing_state) - node_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/actuator_driver_rudder b/src/sailing_robot/scripts/actuator_driver_rudder index 53a2d652..61df581b 100755 --- a/src/sailing_robot/scripts/actuator_driver_rudder +++ b/src/sailing_robot/scripts/actuator_driver_rudder @@ -1,60 +1,83 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """Control the rudder servo Subscribes: rudder_control (Int16) """ import time -import pigpio -import rospy -from std_msgs.msg import UInt16, Int16 -import numpy as np - -rudderdata = rospy.get_param('rudder') -rudderservo_PWM_offset = rudderdata['PWMoffset'] -rudderservo_lower_limits = rudderdata['servolowerlimits'] -rudderservo_higher_limits = rudderdata['servohigherlimits'] -rudderservo_netural_point = (rudderservo_lower_limits + - rudderservo_higher_limits) / 2 -rudderservo_range = (rudderservo_higher_limits - rudderservo_lower_limits) - -PIN = rospy.get_param('rudder/pin') - -def setup(): - pi = pigpio.pi() - pi.set_mode(13, pigpio.OUTPUT) # GPIO 13/RPi PIN 33 as rudder servo pin - -def rudderservoPWMcontrol(data): - """This function takes in the /rudder_control (90 to -90) value and directly write PWM signal to the rudder servo. Netural point was determined by the start and end points. rudderservoPWMoffset is used for software level trim. """ - degrees = data.data - pwm = rudderservo_range*(-1.0*degrees)/90 + rudderservo_netural_point +\ - rudderservo_PWM_offset - pi.set_servo_pulsewidth(PIN, pwm) - -def post(): - '''Power-On Self Test''' - if not rospy.get_param('do_post', False): - return - - rospy.logwarn('rudder test: lower limit') - for _ in range(4): - pi.set_servo_pulsewidth(PIN, rudderservo_netural_point) - time.sleep(0.25) - pi.set_servo_pulsewidth(PIN, rudderservo_lower_limits) - time.sleep(0.25) - - rospy.logwarn('rudder test: higher limit') - for _ in range(4): - pi.set_servo_pulsewidth(PIN, rudderservo_netural_point) - time.sleep(0.25) - pi.set_servo_pulsewidth(PIN, rudderservo_higher_limits) - time.sleep(0.25) +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int16 -if __name__ == '__main__': - pi = pigpio.pi() - post() +try: + import pigpio + PIGPIO_AVAILABLE = True +except ImportError: + PIGPIO_AVAILABLE = False + + +class ActuatorDriverRudder(Node): + def __init__(self): + super().__init__('actuator_driver_rudder') + + self.declare_parameter('rudder.PWMoffset', 0.0) + self.declare_parameter('rudder.servolowerlimits', 1000.0) + self.declare_parameter('rudder.servohigherlimits', 2000.0) + self.declare_parameter('rudder.maxAngle', 40.0) + self.declare_parameter('rudder.pin', 13) + self.declare_parameter('do_post', False) + + self.pwm_offset = self.get_parameter('rudder.PWMoffset').value + self.lower_limits = self.get_parameter('rudder.servolowerlimits').value + self.higher_limits = self.get_parameter('rudder.servohigherlimits').value + self.neutral = (self.lower_limits + self.higher_limits) / 2 + self.range = self.higher_limits - self.lower_limits + self.pin = self.get_parameter('rudder.pin').value + do_post = self.get_parameter('do_post').value + + if PIGPIO_AVAILABLE: + self.pi = pigpio.pi() + self.pi.set_mode(self.pin, pigpio.OUTPUT) + if do_post: + self.post() + else: + self.pi = None + self.get_logger().warn('pigpio not available - running in simulation mode') + + self.create_subscription(Int16, 'rudder_control', self.rudder_servo_pwm_control, 10) + + def rudder_servo_pwm_control(self, data): + degrees = data.data + pwm = self.range * (-1.0 * degrees) / 90 + self.neutral + self.pwm_offset + if self.pi: + self.pi.set_servo_pulsewidth(self.pin, pwm) + + def post(self): + self.get_logger().warn('rudder test: lower limit') + for _ in range(4): + self.pi.set_servo_pulsewidth(self.pin, self.neutral) + time.sleep(0.25) + self.pi.set_servo_pulsewidth(self.pin, self.lower_limits) + time.sleep(0.25) + + self.get_logger().warn('rudder test: higher limit') + for _ in range(4): + self.pi.set_servo_pulsewidth(self.pin, self.neutral) + time.sleep(0.25) + self.pi.set_servo_pulsewidth(self.pin, self.higher_limits) + time.sleep(0.25) + + +def main(args=None): + rclpy.init(args=args) + node = ActuatorDriverRudder() try: - rospy.init_node('actuator_driver_servos', anonymous=True) - rospy.Subscriber('rudder_control', Int16, rudderservoPWMcontrol) - rospy.spin() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/actuator_driver_sail b/src/sailing_robot/scripts/actuator_driver_sail index bfc9d4e4..8682111d 100755 --- a/src/sailing_robot/scripts/actuator_driver_sail +++ b/src/sailing_robot/scripts/actuator_driver_sail @@ -1,51 +1,76 @@ -#!/usr/bin/env python -""" -Node looks up correct sail setting from table -Subscribes: wind direction apparent -Sets sail actuator to correct PWM value +#!/usr/bin/env python3 +"""Control the sail servo + +Subscribes: sailsheet_normalized (Float32) """ import time -import rospy -from std_msgs.msg import Float64, Float32, String -import pigpio -from sailing_robot.sail_table import SailTable +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 -SAILGPIO = rospy.get_param('sail/pin') +try: + import pigpio + PIGPIO_AVAILABLE = True +except ImportError: + PIGPIO_AVAILABLE = False -# get dictionary for the boat specific sail PWM settings -minPWM = rospy.get_param('sail/servolowerlimits') -maxPWM = rospy.get_param('sail/servohigherlimits') -def sail_servo_update(msg): - sheet_normalized = msg.data +class ActuatorDriverSail(Node): + def __init__(self): + super().__init__('actuator_driver_sail') - # calculate actual PWM value from limits - sheetPWM = (sheet_normalized * (maxPWM-minPWM)) + minPWM - debug_pub_pwm.publish(sheetPWM) - piPWM.set_servo_pulsewidth(SAILGPIO, sheetPWM) + self.declare_parameter('sail.pin', 24) + self.declare_parameter('sail.servolowerlimits', 1000.0) + self.declare_parameter('sail.servohigherlimits', 2000.0) + self.declare_parameter('do_post', False) -def post(): - '''Power-On Self Test''' - if not rospy.get_param('do_post', False): - pass + self.sail_gpio = self.get_parameter('sail.pin').value + self.min_pwm = self.get_parameter('sail.servolowerlimits').value + self.max_pwm = self.get_parameter('sail.servohigherlimits').value + do_post = self.get_parameter('do_post').value - rospy.logwarn('sail test: sheet in') - piPWM.set_servo_pulsewidth(SAILGPIO, minPWM) - time.sleep(3) - rospy.logwarn('sail test: sheet out') - piPWM.set_servo_pulsewidth(SAILGPIO, maxPWM) - time.sleep(3) + self.debug_pub = self.create_publisher(Float32, 'debug_sailsheet_pwm', 10) -if __name__ == '__main__': - piPWM = pigpio.pi(); - piPWM.set_mode(SAILGPIO, pigpio.OUTPUT) # GPIO 24/RPi PIN 18 as sail servo pin - post() - try: - debug_pub_pwm = rospy.Publisher('debug_sailsheet_pwm', Float32, queue_size=10) - rospy.init_node('actuator_driver_sail', anonymous=True) - rospy.Subscriber('sailsheet_normalized', Float32, sail_servo_update) + if PIGPIO_AVAILABLE: + self.pi_pwm = pigpio.pi() + self.pi_pwm.set_mode(self.sail_gpio, pigpio.OUTPUT) + if do_post: + self.post() + else: + self.pi_pwm = None + self.get_logger().warn('pigpio not available - running in simulation mode') + + self.create_subscription(Float32, 'sailsheet_normalized', self.sail_servo_update, 10) - rospy.spin() + def sail_servo_update(self, msg): + sheet_normalized = msg.data + sheet_pwm = (sheet_normalized * (self.max_pwm - self.min_pwm)) + self.min_pwm + pwm_msg = Float32() + pwm_msg.data = float(sheet_pwm) + self.debug_pub.publish(pwm_msg) + if self.pi_pwm: + self.pi_pwm.set_servo_pulsewidth(self.sail_gpio, sheet_pwm) - except rospy.ROSInterruptException: + def post(self): + self.get_logger().warn('sail test: sheet in') + self.pi_pwm.set_servo_pulsewidth(self.sail_gpio, self.min_pwm) + time.sleep(3) + self.get_logger().warn('sail test: sheet out') + self.pi_pwm.set_servo_pulsewidth(self.sail_gpio, self.max_pwm) + time.sleep(3) + + +def main(args=None): + rclpy.init(args=args) + node = ActuatorDriverSail() + try: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/debugging_2D_plot b/src/sailing_robot/scripts/debugging_2D_plot index d735f02f..fe4f57d8 100755 --- a/src/sailing_robot/scripts/debugging_2D_plot +++ b/src/sailing_robot/scripts/debugging_2D_plot @@ -1,207 +1,79 @@ -#!/usr/bin/env python -# READY FOR MIT +#!/usr/bin/env python3 +"""Node that publishes visualisation objects (for RViz).""" -from visualization_msgs.msg import Marker -from visualization_msgs.msg import MarkerArray +import math +import rclpy +from rclpy.node import Node +from visualization_msgs.msg import Marker, MarkerArray from sensor_msgs.msg import NavSatFix from std_msgs.msg import Float64, Float32, String, Bool - from sailing_robot.navigation import Navigation -import rospy -import math -import time -from shapely.geometry import Polygon - -class Debugging_2D_plot(): - """ - Node that publish visualisation objects (for RViz) - """ +class Debugging2DPlot(Node): def __init__(self): - - self.publisher = rospy.Publisher('debugging_2D_plot', MarkerArray, queue_size=10) - self.publisher_waypoint = rospy.Publisher('debugging_2D_plot_wp', MarkerArray, queue_size=10) - self.publisher_origin = rospy.Publisher('debugging_2D_plot_origin', NavSatFix, queue_size=10) - - rospy.init_node("debugging_2D_plot", anonymous=True) + super().__init__('debugging_2D_plot') - rospy.Subscriber('sailing_state', String, self.update_sailing_state) - self.sailing_state = 'normal' - - rospy.Subscriber('remote_control', String, self.update_remote_control) - self.remote_control = False + self.declare_parameter('navigation.utm_zone', 30) - utm_zone = rospy.get_param('navigation/utm_zone') + utm_zone = self.get_parameter('navigation.utm_zone').value self.nav = Navigation(utm_zone=utm_zone) - rospy.Subscriber('position', NavSatFix, self.update_position) - self.gps_fix_lock = True - self.markerArray = MarkerArray() - self.radius = rospy.get_param('wp/acceptRadius') + self.publisher = self.create_publisher(MarkerArray, 'debugging_2D_plot', 10) + self.publisher_waypoint = self.create_publisher(MarkerArray, 'debugging_2D_plot_wp', 10) + self.publisher_origin = self.create_publisher(NavSatFix, 'debugging_2D_plot_origin', 10) - self.rate = rospy.Rate(rospy.get_param("config/rate")) - - if rospy.has_param('simulation/boatColour/red'): - self.colour_red = rospy.get_param("simulation/boatColour/red") - self.colour_green = rospy.get_param("simulation/boatColour/green") - self.colour_blue = rospy.get_param("simulation/boatColour/blue") - else: - self.colour_red = 256 - self.colour_green = 256 - self.colour_blue = 0 - - self.count = 0 - self._id_counter = 0 - self.MARKERS_MAX = 500 - - while self.gps_fix_lock and not rospy.is_shutdown(): - self.rate.sleep() - self.init_position = self.position - self.init_position_gps = self.position_gps - - - if rospy.has_param('wp/list'): - self.generate_wp_Array(rospy.get_param('wp/list')) - elif rospy.has_param('wp/tasks'): - tasks_list = rospy.get_param('wp/tasks') - wp_list = [t['waypoint'] for t in tasks_list if 'waypoint' in t] - self.generate_wp_Array(wp_list) - else: - rospy.logwarn("No waypoint list found in parameters") - self.marker_publish() - - - def generate_wp_Array(self, wp_list): - wp_table = rospy.get_param('wp/table') - - wp_list = list(set(wp_list)) # only print each waypoint once - - self.wp_Array = MarkerArray() + self.sailing_state = 'normal' + self.remote_control = False + self.gps_position = None - for key in wp_table: - wp_coord = wp_table[key] - if key in wp_list: - rgb = (200.0/255.0, 162.0/255.0, 200.0/255.0) - else: - rgb = (1.0, 1.0, 1.0) - current_wp = self.make_marker(wp_coord, rgb) - self.wp_Array.markers.append(current_wp) + self.create_subscription(String, 'sailing_state', self.update_sailing_state, 10) + self.create_subscription(String, 'remote_control', self.update_remote_control, 10) + self.create_subscription(NavSatFix, 'position', self.update_position, 10) - try: - limit_coords = rospy.get_param('navigation/safety_zone_ll') - except KeyError: - return - - for latlon in limit_coords: - rgb = (162.0/255.0, 200.0/255.0, 200.0/255.0) - limit_marker = self.make_marker(latlon, rgb) - self.wp_Array.markers.append(limit_marker) - - corners = [self.nav.latlon_to_utm(*l) for l in limit_coords] - p = Polygon(corners) - centre_marker = self.make_marker((p.centroid.x, p.centroid.y), (1.0, 1.0, 0.6)) - self.wp_Array.markers.append(centre_marker) - - - def make_marker(self, wp_coord, rgb): - current_wp_position = self.nav.latlon_to_utm(wp_coord[0], wp_coord[1]) - current_wp = Marker() - - current_wp.header.frame_id = "map" - current_wp.type = current_wp.SPHERE - current_wp.action = current_wp.ADD - current_wp.scale.x = self.radius - current_wp.scale.y = self.radius - current_wp.scale.z = self.radius - - current_wp.color.a = 0.9 - current_wp.color.r, current_wp.color.g, current_wp.color.b = rgb - current_wp.pose.orientation.w = 1.0 - current_wp.pose.position.x = current_wp_position[0] - self.init_position[0] - current_wp.pose.position.y = current_wp_position[1] - self.init_position[1] - current_wp.pose.position.z = 0 - current_wp.id = self._id_counter - self._id_counter += 1 - - return current_wp + self.timer = self.create_timer(1.0, self.publish) def update_sailing_state(self, msg): self.sailing_state = msg.data - def update_position(self, msg): - self.position = self.nav.latlon_to_utm( msg.latitude, msg.longitude) - self.position_gps = msg - self.gps_fix_lock = False - def update_remote_control(self, msg): self.remote_control = msg.data - def marker_publish(self): - - while not rospy.is_shutdown(): - marker = Marker() - marker.header.frame_id = "map" - marker.type = marker.SPHERE - marker.action = marker.ADD - marker.scale.x = 2 - marker.scale.y = 2 - marker.scale.z = 2 - - if self.remote_control: - marker.color.a = 0.8 - marker.color.r = 0.5 - marker.color.g = 0.5 - marker.color.b = 0.5 - elif self.sailing_state == 'switch_to_port_tack': - marker.color.a = 1.0 - marker.color.r = 1.0 - marker.color.g = 0.0 - marker.color.b = 0.0 - elif self.sailing_state == 'switch_to_stbd_tack': - marker.color.a = 1.0 - marker.color.r = 0.0 - marker.color.g = 1.0 - marker.color.b = 0.0 - else: - marker.color.a = 1.0 - marker.color.r = self.colour_red /256.0 - marker.color.g = self.colour_green /256.0 - marker.color.b = self.colour_blue /256.0 - - marker.pose.orientation.w = 1.0 - marker.pose.position.x = self.position[0] - self.init_position[0] - marker.pose.position.y = self.position[1] - self.init_position[1] - - marker.pose.position.z = 0 - - # We add the new marker to the MarkerArray, removing the oldest - # marker from it when necessary - if(self.count > self.MARKERS_MAX): - self.markerArray.markers.pop(0) - - self.markerArray.markers.append(marker) - - # Renumber the marker IDs - id = 0 - for m in self.markerArray.markers: - m.id = id - id += 1 - - - - self.publisher.publish(self.markerArray) - self.publisher_waypoint.publish(self.wp_Array) - self.publisher_origin.publish(self.init_position_gps) + def update_position(self, msg): + self.nav.update_position(msg) + self.gps_position = msg - self.count += 1 + def publish(self): + if self.gps_position is None: + return + marker_array = MarkerArray() + marker = Marker() + marker.header.frame_id = 'map' + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.scale.x = 2.0 + marker.scale.y = 2.0 + marker.scale.z = 2.0 + marker.color.a = 1.0 + if self.sailing_state == 'normal': + marker.color.g = 1.0 + else: + marker.color.r = 1.0 + marker_array.markers.append(marker) + self.publisher.publish(marker_array) - self.rate.sleep() +def main(args=None): + rclpy.init(args=args) + node = Debugging2DPlot() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - Debugging_2D_plot() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/debugging_2D_plot_matplot b/src/sailing_robot/scripts/debugging_2D_plot_matplot index 35e8f02d..69f7edcd 100755 --- a/src/sailing_robot/scripts/debugging_2D_plot_matplot +++ b/src/sailing_robot/scripts/debugging_2D_plot_matplot @@ -1,81 +1,97 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import os -from sailing_robot.navigation import Navigation -import rospy, math, time, collections -import matplotlib.pyplot as plt -import matplotlib.animation as animation -import matplotlib.image as mpimg - +import math +import collections +import numpy as np +import rclpy +from rclpy.node import Node from std_msgs.msg import String, Float32, Float64 from sensor_msgs.msg import NavSatFix -import numpy as np -#import smopy +from sailing_robot.navigation import Navigation + +try: + import matplotlib.pyplot as plt + import matplotlib.animation as animation + import matplotlib.image as mpimg + MATPLOTLIB_AVAILABLE = True +except ImportError: + MATPLOTLIB_AVAILABLE = False -# color palette definition (V2 from https://matplotlib.org/users/dflt_style_changes.html#colors-color-cycles-and-color-maps) +# colour palette (V2 from matplotlib dflt_style_changes) C = ['#1f77b4', '#ff7f0e', '#2ca02c', '#d62728', '#9467bd', '#8c564b', '#e377c2', '#7f7f7f', '#bcbd22', '#17becf'] -class Debugging_2D_matplot(): - +class Debugging2DMatplot(Node): def __init__(self): - rospy.init_node("debugging_2D_matplot") + super().__init__('debugging_2D_matplot') - utm_zone = rospy.get_param('navigation/utm_zone') - self.nav = Navigation(utm_zone=utm_zone) + self.declare_parameter('navigation.utm_zone', 30) + self.declare_parameter('config.rate', 10) + self.declare_parameter('wp.acceptRadius', 2.5) + self.declare_parameter('wp.list', ['wp0']) + self.declare_parameter('wp.table', '{}') - self.rate = rospy.Rate(rospy.get_param("config/rate")) - self.wp_radius = rospy.get_param('wp/acceptRadius') + utm_zone = self.get_parameter('navigation.utm_zone').value + self.wp_radius = self.get_parameter('wp.acceptRadius').value - # Get waypoints - if rospy.has_param('wp/list'): - wp_list = rospy.get_param('wp/list') - elif rospy.has_param('wp/tasks'): - tasks_list = rospy.get_param('wp/tasks') - wp_list = [t['waypoint'] for t in tasks_list if 'waypoint' in t] + wp_table_val = self.get_parameter('wp.table').value + if isinstance(wp_table_val, str): + import yaml + wp_table = yaml.safe_load(wp_table_val) or {} else: - rospy.logwarn("No waypoint was found!") - - wp_table = rospy.get_param('wp/table') - wp_list = list(set(wp_list)) # print each point only once - self.wp_array = np.array([self.nav.latlon_to_utm(wp_table[wp][0], wp_table[wp][1]) for wp in wp_list]).T # [lat, lon] + wp_table = wp_table_val or {} - self.origin = [self.wp_array[0].mean(), self.wp_array[1].mean()] - - # Subscribers init - rospy.Subscriber('sailing_state', String, self.update_sailing_state) - self.sailing_state = 'normal' + wp_list_val = self.get_parameter('wp.list').value + if not isinstance(wp_list_val, list): + wp_list_val = [wp_list_val] + wp_list = list(set(wp_list_val)) - rospy.Subscriber('heading', Float32, self.update_heading) - self.heading = 0 - - rospy.Subscriber('goal_heading', Float32, self.update_goal_heading) - self.goal_heading = 0 - - rospy.Subscriber('wind_direction_apparent', Float64, self.update_wind_direction) - self.wind_boat = 0 - self.wind_north = 0 + self.nav = Navigation(utm_zone=utm_zone) - self.position_history = collections.deque(maxlen = 500) - rospy.Subscriber('position', NavSatFix, self.update_position) - self.position = [1,1] + if wp_table and wp_list: + self.wp_array = np.array([ + self.nav.latlon_to_utm(wp_table[wp][0], wp_table[wp][1]) + for wp in wp_list if wp in wp_table + ]).T + else: + self.wp_array = np.array([[0.0], [0.0]]) - self.window = [0,0,0,0] - self.init_plot() - self.update_plot() + self.origin = [float(self.wp_array[0].mean()), float(self.wp_array[1].mean())] + self.sailing_state = 'normal' + self.heading = 0.0 + self.goal_heading = 0.0 + self.wind_boat = 0.0 + self.wind_north = 0.0 + self.position = [0.0, 0.0] + self.position_history = collections.deque(maxlen=500) + self.window = [0, 0, 0, 0] + + self.create_subscription(String, 'sailing_state', self.update_sailing_state, 10) + self.create_subscription(Float32, 'heading', self.update_heading, 10) + self.create_subscription(Float32, 'goal_heading', self.update_goal_heading, 10) + self.create_subscription(Float64, 'wind_direction_apparent', self.update_wind_direction, 10) + self.create_subscription(NavSatFix, 'position', self.update_position, 10) + + if MATPLOTLIB_AVAILABLE: + self.init_plot() + self._start_animation() + else: + self.get_logger().warn('matplotlib not available — plot node running without display') def update_sailing_state(self, msg): self.sailing_state = msg.data def update_position(self, msg): - self.position = list(self.nav.latlon_to_utm(msg.latitude, msg.longitude)) - self.position[0] -= self.origin[0] - self.position[1] -= self.origin[1] - self.position_history.append(self.position) + pos = list(self.nav.latlon_to_utm(msg.latitude, msg.longitude)) + pos[0] -= self.origin[0] + pos[1] -= self.origin[1] + self.position = pos + self.position_history.append(list(pos)) def update_heading(self, msg): self.heading = msg.data @@ -89,176 +105,154 @@ class Debugging_2D_matplot(): self.wind_north = np.radians(self.heading + self.wind_boat) def get_bg_image(self): - self.has_bg_img = False self.image = None - side_dist = None + side_dist = None image_origin = None my_dir = os.path.dirname(__file__) - image_dir = os.path.abspath(os.path.join(my_dir, '../../../utilities/map_bg_images')) + image_dir = os.path.abspath( + os.path.join(my_dir, '../../../utilities/map_bg_images')) + + if not os.path.isdir(image_dir): + return - # select the correct image if any for filename in os.listdir(image_dir): - if not filename.endswith(".png"): + if not filename.endswith('.png'): + continue + data_filename = filename[:-4].split('_') + try: + current_img_origin = self.nav.latlon_to_utm( + float(data_filename[0]), float(data_filename[1])) + except (IndexError, ValueError): continue - data_filename = filename[:-4].split("_") - current_img_origin = self.nav.latlon_to_utm(float(data_filename[0]), float(data_filename[1])) + dist_to_origin = math.sqrt( + (self.origin[0] - current_img_origin[0]) ** 2 + + (self.origin[1] - current_img_origin[1]) ** 2) - dist_to_origin = ((self.origin[0] - current_img_origin[0])**2 + - (self.origin[1] - current_img_origin[1])**2)**0.5 - if dist_to_origin < 1000: - image_origin = current_img_origin + image_origin = current_img_origin side_dist = float(data_filename[2]) - self.image = mpimg.imread(os.path.join(image_dir,filename)) + self.image = mpimg.imread(os.path.join(image_dir, filename)) self.has_bg_img = True break - + if not self.has_bg_img: return - + minx = image_origin[0] - side_dist - self.origin[0] maxx = image_origin[0] + side_dist - self.origin[0] miny = image_origin[1] - side_dist - self.origin[1] maxy = image_origin[1] + side_dist - self.origin[1] - - image_size = (minx, maxx, miny, maxy) - self.image_show = self.ax.imshow(self.image, extent=image_size) - + self.image_show = self.ax.imshow(self.image, extent=(minx, maxx, miny, maxy)) def init_plot(self): - - # recenter wp to the origin - for i, _ in enumerate(self.wp_array[0]): + for i in range(len(self.wp_array[0])): self.wp_array[0][i] -= self.origin[0] self.wp_array[1][i] -= self.origin[1] - self.maxwpdist = [0,0] - self.maxwpdist[0] = np.max(np.abs(self.wp_array[0])) - self.maxwpdist[1] = np.max(np.abs(self.wp_array[1])) + self.maxwpdist = [ + float(np.max(np.abs(self.wp_array[0]))), + float(np.max(np.abs(self.wp_array[1]))), + ] self.fig = plt.figure() - self.boatline, = plt.plot([], [], c=C[0], label="Heading") - plt.plot([], [], c=C[7], label="Goal heading") - plt.plot([], [], c=C[1], label="Wind direction") - - # display Waypoints + self.ax = plt.subplot(111) + self.boatline, = plt.plot([], [], c=C[0], label='Heading') + plt.plot([], [], c=C[7], label='Goal heading') + plt.plot([], [], c=C[1], label='Wind direction') self.wpfig = plt.scatter(self.wp_array[0], self.wp_array[1], c=C[3]) - plt.tight_layout() - self.ax = plt.subplot(111) - self.get_bg_image() - def get_arrow(self, angle, color, reverse=False): - figsize = self.fig.get_size_inches() - scale_dx = figsize[0]/np.sqrt(figsize[0]**2 + figsize[1]**2) - scale_dy = figsize[1]/np.sqrt(figsize[0]**2 + figsize[1]**2) - - if reverse: - style = '<-' - else: - style = '->' - + scale_dx = figsize[0] / np.sqrt(figsize[0] ** 2 + figsize[1] ** 2) + scale_dy = figsize[1] / np.sqrt(figsize[0] ** 2 + figsize[1] ** 2) + style = '<-' if reverse else '->' arrow_ori = (0.88, 0.12) - arrow_target = (arrow_ori[0] + 0.05*np.sin(angle)/scale_dx, arrow_ori[1] + 0.05*np.cos(angle)/scale_dy) - arrow = self.ax.annotate("", - xy=arrow_target, xycoords=self.ax.transAxes, - xytext=arrow_ori, textcoords=self.ax.transAxes, - arrowprops=dict(arrowstyle=style, - color=color, - connectionstyle="arc3"), - ) - - return arrow - - + arrow_target = ( + arrow_ori[0] + 0.05 * np.sin(angle) / scale_dx, + arrow_ori[1] + 0.05 * np.cos(angle) / scale_dy, + ) + return self.ax.annotate( + '', + xy=arrow_target, xycoords=self.ax.transAxes, + xytext=arrow_ori, textcoords=self.ax.transAxes, + arrowprops=dict(arrowstyle=style, color=color, + connectionstyle='arc3'), + ) def animate(self, i): if self.position_history: - lat, lon = np.array(self.position_history).T - self.boatline.set_data(lat,lon) + lat, lon = np.array(list(self.position_history)).T + self.boatline.set_data(lat, lon) wind_arrow = self.get_arrow(self.wind_north, C[1], reverse=True) - heading_arrow = self.get_arrow(np.radians(self.heading), C[0]) - goal_heading_arrow = self.get_arrow(np.radians(self.goal_heading), C[7]) - arrow_col = C[0] - if self.sailing_state != 'normal': - arrow_col = C[1] - arrow_dx = 0.1*np.sin(np.radians(self.heading)) - arrow_dy = 0.1*np.cos(np.radians(self.heading)) - boat_arrow = plt.arrow(self.position[0] - arrow_dx, self.position[1] - arrow_dy, - arrow_dx, arrow_dy, - head_width=0.5, - head_length=1., - fc=arrow_col, - ec=arrow_col) + arrow_col = C[1] if self.sailing_state != 'normal' else C[0] + arrow_dx = 0.1 * np.sin(np.radians(self.heading)) + arrow_dy = 0.1 * np.cos(np.radians(self.heading)) + boat_arrow = plt.arrow( + self.position[0] - arrow_dx, self.position[1] - arrow_dy, + arrow_dx, arrow_dy, + head_width=0.5, head_length=1.0, + fc=arrow_col, ec=arrow_col) self.update_window(i) + artists = [self.boatline, wind_arrow, boat_arrow, self.wpfig, + goal_heading_arrow, heading_arrow, plt.legend()] if self.has_bg_img: - return self.image_show, self.boatline, wind_arrow, boat_arrow, self.wpfig, goal_heading_arrow, heading_arrow, plt.legend() - else: - return self.boatline, wind_arrow, boat_arrow, self.wpfig, goal_heading_arrow, heading_arrow, plt.legend() - + artists.insert(0, self.image_show) + return tuple(artists) def update_window(self, i): - # update window only every second - if not i%10 == 0: - return - - # rounding of the window size in m + if i % 10 != 0: + return rounding = 10.0 + distx = max(self.maxwpdist[0], abs(self.position[0])) + rounding / 2 + disty = max(self.maxwpdist[1], abs(self.position[1])) + rounding / 2 + distx = int(round(distx / rounding) * rounding) + 1 + disty = int(round(disty / rounding) * rounding) + 1 - # maximum distance to origin in both direction - distx = max(self.maxwpdist[0], abs(self.position[0])) + rounding/2 - disty = max(self.maxwpdist[1], abs(self.position[1])) + rounding/2 - - distx = int(round(distx/rounding)*rounding) + 1 - disty = int(round(disty/rounding)*rounding) + 1 - - # scaling to keep x and y orthonormal figsize = self.fig.get_size_inches() - scale_dx = figsize[0]/np.sqrt(figsize[0]**2 + figsize[1]**2) - scale_dy = figsize[1]/np.sqrt(figsize[0]**2 + figsize[1]**2) + scale_dx = figsize[0] / np.sqrt(figsize[0] ** 2 + figsize[1] ** 2) + scale_dy = figsize[1] / np.sqrt(figsize[0] ** 2 + figsize[1] ** 2) norm = min(scale_dx, scale_dy) - scale_dx = scale_dx/norm - scale_dy = scale_dy/norm - - # decide which axis is the limiting one - if distx*scale_dy > disty*scale_dx: - dist = distx - else: - dist = disty - - minx = - dist * scale_dx - maxx = + dist * scale_dx - miny = - dist * scale_dy - maxy = + dist * scale_dy - - if self.window == [minx, maxx, miny, maxy]: - return - - self.window = [minx, maxx, miny, maxy] - self.ax.axis(self.window) - - return + scale_dx /= norm + scale_dy /= norm + + dist = distx if distx * scale_dy > disty * scale_dx else disty + window = [ + -dist * scale_dx, dist * scale_dx, + -dist * scale_dy, dist * scale_dy, + ] + if window != self.window: + self.window = window + self.ax.axis(window) + + def _start_animation(self): + # Keep a reference to prevent the animation from being garbage-collected + self._line_ani = animation.FuncAnimation( + self.fig, self.animate, interval=100, blit=True) + plt.show() +def main(args=None): + rclpy.init(args=args) + node = Debugging2DMatplot() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() - def update_plot(self): - line_ani = animation.FuncAnimation(self.fig, self.animate, - interval=100, blit=True) - plt.show() if __name__ == '__main__': - try : - Debugging_2D_matplot() - except rospy.ROSInterruptException: - pass + main() + diff --git a/src/sailing_robot/scripts/debugging_blink_on_sailing_state b/src/sailing_robot/scripts/debugging_blink_on_sailing_state index 8e5caef8..bb6acc74 100755 --- a/src/sailing_robot/scripts/debugging_blink_on_sailing_state +++ b/src/sailing_robot/scripts/debugging_blink_on_sailing_state @@ -1,49 +1,47 @@ -#!/usr/bin/python -# READY FOR MIT -""" -Blink LEDs to indicate sailing state -\n -Publishes: messages that encode a colour in an Int32 -\n -Subscribes: sailing state -""" - -import rospy +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node from std_msgs.msg import Int32, String -class Blink_on_sailing_state(): +class BlinkOnSailingState(Node): def __init__(self): - self.led_blink = rospy.Publisher('led_blink', Int32, queue_size=10) + super().__init__('debugging_blink_on_sailing_state') + + self.declare_parameter('config.rate', 10) + rate_hz = self.get_parameter('config.rate').value - rospy.init_node("debugging_blink_on_sailing_state", anonymous=True) - rospy.Subscriber('sailing_state', String, self.update_sailing_state) - self.sailing_state = "normal" - self.rate = rospy.Rate(rospy.get_param("config/rate")) - self.blink_publisher() + self.sailing_state = 'normal' + self.led_blink = self.create_publisher(Int32, 'led_blink', 10) + self.create_subscription(String, 'sailing_state', self.update_sailing_state, 10) + self.timer = self.create_timer(1.0 / rate_hz, self.blink_publisher) def update_sailing_state(self, msg): self.sailing_state = msg.data - def blink_publisher(self): - - while not rospy.is_shutdown(): - - if self.sailing_state == "switch_to_port_tack": - color = 255*300*300 # red - self.led_blink.publish(color) - - elif self.sailing_state == "switch_to_stbd_tack": - color = 255*300 # green - self.led_blink.publish(color) - - self.rate.sleep() + msg = Int32() + if self.sailing_state == 'switch_to_port_tack': + msg.data = 255 * 300 * 300 # red + self.led_blink.publish(msg) + elif self.sailing_state == 'switch_to_stbd_tack': + msg.data = 255 * 300 # green + self.led_blink.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = BlinkOnSailingState() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - Blink_on_sailing_state() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/debugging_dashboard b/src/sailing_robot/scripts/debugging_dashboard index 07d65a6a..e76748ca 100755 --- a/src/sailing_robot/scripts/debugging_dashboard +++ b/src/sailing_robot/scripts/debugging_dashboard @@ -1,124 +1,101 @@ -#!/usr/bin/env python -""" -Supply browser view of ros topics in local network -""" +#!/usr/bin/env python3 +"""Supply browser view of ros topics in local network.""" -import rospy +import json +import os +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32, Float64, Int16, String, Bool from sensor_msgs.msg import NavSatFix -from rosgraph_msgs.msg import Log -import json -import os -import tornado.ioloop -import tornado.log -import tornado.web -import tornado.websocket +try: + import tornado.ioloop + import tornado.web + import tornado.websocket + TORNADO_AVAILABLE = True +except ImportError: + TORNADO_AVAILABLE = False PORT = 8448 STATIC_PATH = os.path.expanduser('~/sailing-robot/dashboard/src') -simple_topics = [ - ('/heading', Float32), - ('/heading_comp', Float32), - # ('/pitch', Float32), - # ('/roll', Float32), - ('/goal_heading', Float32), - ('/dbg_goal_wind_angle', Float32), - ('/rudder_control', Int16), - ('/sailing_state', String), - ('/dbg_helming_procedure', String), - ('/wind_direction_apparent', Float64), - ('/dbg_distance_to_waypoint', Float32), - ('/dbg_heading_to_waypoint', Float32), - ('/gps_satellites', Int16), - # ('/tack_rudder', Float32), - ('/task_ix', Int16), - ('/active_task_kind', String), - ('/dbg_latest_waypoint_id', String), - ('/wind_direction_average', Float32), - ('/camera_detection', String), - ('/remote_control', Bool), +SIMPLE_TOPICS = [ + ('heading', Float32), + ('goal_heading', Float32), + ('rudder_control', Int16), + ('sailing_state', String), + ('dbg_helming_procedure', String), + ('wind_direction_apparent', Float64), + ('dbg_distance_to_waypoint', Float32), + ('dbg_heading_to_waypoint', Float32), + ('gps_satellites', Int16), + ('task_ix', Int16), + ('active_task_kind', String), + ('wind_direction_average', Float32), + ('camera_detection', String), ] -geo_topics = [ - # '/position', -] -class MessageForwarder(object): - """Forward ROS messages to a websocket""" +class DebuggingDashboard(Node): def __init__(self): - self.sockets = [] - self.loop = tornado.ioloop.IOLoop.current() - for topic, rostype in simple_topics: - self.simple_forwarding(topic, rostype) - for topic in geo_topics: - self.geo_forwarding(topic) - self.rosout_forwarding() - - def broadcast(self, content): - for s in self.sockets: - s.send_json_message(content) - - # The callbacks we give to ROS are called on separate threads, so we use - # loop.add_callback to ensure we actually send the data from the main thread. - def simple_forwarding(self, topic_name, rostype): - def forward(msg): - self.broadcast({'topic': topic_name, 'value': msg.data}) - return rospy.Subscriber(topic_name, rostype, - lambda m: self.loop.add_callback(forward, m)) - - def geo_forwarding(self, topic_name): - def forward(msg): - self.broadcast({'topic': topic_name, 'latitude': msg.latitude, - 'longitude': msg.longitude}) - return rospy.Subscriber(topic_name, NavSatFix, - lambda m: self.loop.add_callback(forward, m)) - - def rosout_forwarding(self): - def forward(msg): - d = {'topic': '/rosout', 'level': msg.level, 'name': msg.name, - 'msg': msg.msg, 'file': msg.file, 'line': msg.line, - 'function': msg.function, 'topics': msg.topics} - self.broadcast(d) - return rospy.Subscriber('rosout_agg', Log, - lambda m: self.loop.add_callback(forward, m)) - -class UpdateHandler(tornado.websocket.WebSocketHandler): - def initialize(self, forwarder): - self.forwarder = forwarder - - def open(self): - self.forwarder.sockets.append(self) - - def on_close(self): - self.forwarder.sockets.remove(self) - - def send_json_message(self, content): - json_msg = json.dumps(content) - self.write_message(json_msg) - -def dashboard_server(): - tornado.log.enable_pretty_logging() - forwarder = MessageForwarder() - app = tornado.web.Application([ - (r"/updates", UpdateHandler, - {"forwarder": forwarder}), - (r"/(.*)", tornado.web.StaticFileHandler, - {"path": STATIC_PATH, "default_filename": "index.html"}), - - ], - compiled_template_cache=False, - ) - app.listen(PORT, address='0.0.0.0') - loop = tornado.ioloop.IOLoop.current() - rospy.client.on_shutdown(loop.stop) - loop.start() + super().__init__('debugging_dashboard') + self.data = {} + for topic, msg_type in SIMPLE_TOPICS: + self.data[topic] = None + self.create_subscription( + msg_type, topic, + lambda msg, t=topic: self._update(t, msg), + 10 + ) -if __name__ == '__main__': + self.create_subscription(NavSatFix, 'position', self._update_position, 10) + + if TORNADO_AVAILABLE: + self.get_logger().info('Dashboard available at http://localhost:%d' % PORT) + self._start_server() + else: + self.get_logger().warn('tornado not available - dashboard disabled') + + def _update(self, topic, msg): + self.data[topic] = msg.data + + def _update_position(self, msg): + self.data['position'] = {'lat': msg.latitude, 'lon': msg.longitude} + + def _start_server(self): + dashboard_data = self.data + + class DataHandler(tornado.websocket.WebSocketHandler): + def open(self): + self.write_message(json.dumps(dashboard_data, default=str)) + + def on_message(self, message): + pass + + class IndexHandler(tornado.web.RequestHandler): + def get(self): + self.write('Sailing Robot Dashboard') + + app = tornado.web.Application([ + (r'/', IndexHandler), + (r'/ws', DataHandler), + (r'/static/(.*)', tornado.web.StaticFileHandler, {'path': STATIC_PATH}), + ]) + app.listen(PORT) + + +def main(args=None): + rclpy.init(args=args) + node = DebuggingDashboard() try: - rospy.init_node("debugging_dashboard", anonymous=True) - dashboard_server() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/debugging_dump_params b/src/sailing_robot/scripts/debugging_dump_params index c45acb61..7f702f1c 100755 --- a/src/sailing_robot/scripts/debugging_dump_params +++ b/src/sailing_robot/scripts/debugging_dump_params @@ -1,18 +1,48 @@ -#!/usr/bin/env python -"""Dump all parameters, to match up with our rosbag files. -""" +#!/usr/bin/env python3 +"""Dump all parameters, to match up with our rosbag files.""" from datetime import datetime import json import os.path -import rospy -FILENAME_BASE = "~/sailing-robot/params-dump_{}_{}.json" +import rclpy +from rclpy.node import Node -params = rospy.get_param('/') +FILENAME_BASE = '~/sailing-robot/params-dump_{}_{}.json' -filename = FILENAME_BASE.format(params.get('log_name', ''), - datetime.now().strftime('%Y-%m-%dT%H.%M.%S')) -filename = os.path.expanduser(filename) -with open(filename, 'w') as f: - json.dump(params, f, indent=2) +class DebuggingDumpParams(Node): + def __init__(self): + super().__init__('debugging_dump_params') + + self.declare_parameter('log_name', '') + log_name = self.get_parameter('log_name').value + + filename = FILENAME_BASE.format( + log_name, datetime.now().strftime('%Y-%m-%dT%H.%M.%S')) + filename = os.path.expanduser(filename) + + params = { + name: self.get_parameter(name).value + for name in self._parameters + } + + with open(filename, 'w') as f: + json.dump(params, f, indent=2, default=str) + + self.get_logger().info('Parameters dumped to %s' % filename) + + +def main(args=None): + rclpy.init(args=args) + node = DebuggingDumpParams() + try: + rclpy.spin_once(node, timeout_sec=1.0) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/debugging_gps_log b/src/sailing_robot/scripts/debugging_gps_log index 754b26ae..10aa6741 100755 --- a/src/sailing_robot/scripts/debugging_gps_log +++ b/src/sailing_robot/scripts/debugging_gps_log @@ -1,35 +1,56 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Log GPS data in the CSV format required by the rules import csv from datetime import datetime import os.path -import rospy -from sensor_msgs.msg import NavSatFix +import rclpy +from rclpy.node import Node from sailing_robot.msg import gpswtime RECORDS_DIR = os.path.expanduser('~/sailing-robot') -log_name = rospy.get_param('/log_name') - -def record(): - filename = 'gps-trace_{}_{}.csv'.format(log_name, - datetime.now().strftime("%Y-%m-%dT%H.%M.%S")) - day_of_month = datetime.now().day - with open(os.path.join(RECORDS_DIR, filename), 'w', 0) as f: - csvw = csv.writer(f) - def write(msg): - ts = '%02d%02d%02d%02d' % (msg.time_h, msg.time_m, msg.time_s, - day_of_month) - lat = int(msg.fix.latitude * 1e7) - lon = int(msg.fix.longitude * 1e7) - csvw.writerow([ts, lat, lon]) - - rospy.Subscriber('gps_fix', gpswtime, write) - rospy.spin() -if __name__ == '__main__': + +class DebuggingGpsLog(Node): + def __init__(self): + super().__init__('debugging_gps_log') + + self.declare_parameter('log_name', '') + log_name = self.get_parameter('log_name').value + + filename = 'gps-trace_{}_{}.csv'.format( + log_name, datetime.now().strftime('%Y-%m-%dT%H.%M.%S')) + self.day_of_month = datetime.now().day + + os.makedirs(RECORDS_DIR, exist_ok=True) + self.csv_file = open(os.path.join(RECORDS_DIR, filename), 'w', newline='') + self.csvw = csv.writer(self.csv_file) + + self.create_subscription(gpswtime, 'gps_fix', self.write_record, 10) + self.get_logger().info(f'GPS logging to {filename}') + + def write_record(self, msg): + ts = '%02d%02d%02d%02d' % (msg.time_h, msg.time_m, msg.time_s, self.day_of_month) + lat = int(msg.fix.latitude * 1e7) + lon = int(msg.fix.longitude * 1e7) + self.csvw.writerow([ts, lat, lon]) + + def destroy_node(self): + self.csv_file.close() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = DebuggingGpsLog() try: - rospy.init_node("debugging_gps_log", anonymous=True) - record() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/dummy_apparent_wind_direction b/src/sailing_robot/scripts/dummy_apparent_wind_direction index 576a640e..8b674367 100755 --- a/src/sailing_robot/scripts/dummy_apparent_wind_direction +++ b/src/sailing_robot/scripts/dummy_apparent_wind_direction @@ -1,27 +1,40 @@ -#!/usr/bin/python -# -# Node publishes apparent wind direction -# DUMMY NODE, at the moment it just keeps publishing a constant wind direction -# as specified in the parameters file +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Float64 -windDirection = rospy.get_param("dummy/wind_direction_apparent") -def wind_direction_apparent_publisher(): +class DummyApparentWindDirection(Node): + def __init__(self): + super().__init__('publish_dummy_wind_data') - rate = rospy.Rate(rospy.get_param("config/rate")) + self.declare_parameter('dummy.wind_direction_apparent', 180.0) + self.declare_parameter('config.rate', 10) - while not rospy.is_shutdown(): - wind_pub.publish(windDirection) - rate.sleep() + self.wind_direction = self.get_parameter('dummy.wind_direction_apparent').value + rate_hz = self.get_parameter('config.rate').value + self.wind_pub = self.create_publisher(Float64, 'wind_direction_apparent', 10) + self.timer = self.create_timer(1.0 / rate_hz, self.publish_wind) -if __name__ == '__main__': + def publish_wind(self): + msg = Float64() + msg.data = float(self.wind_direction) + self.wind_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = DummyApparentWindDirection() try: - wind_pub = rospy.Publisher('wind_direction_apparent', Float64, queue_size=10) - rospy.init_node("publish_dummy_wind_data", anonymous=True) - wind_direction_apparent_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/dummy_heading b/src/sailing_robot/scripts/dummy_heading index 0f04727e..2c04d9c1 100755 --- a/src/sailing_robot/scripts/dummy_heading +++ b/src/sailing_robot/scripts/dummy_heading @@ -1,27 +1,40 @@ -#!/usr/bin/python -# -# Node publishes apparent wind direction -# DUMMY NODE, at the moment it just keeps publishing a constant wind direction -# as specified in the parameters file +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32 -currentHeading = rospy.get_param("dummy/heading") -def heading_publisher(): +class DummyHeading(Node): + def __init__(self): + super().__init__('publish_dummy_heading_data') - rate = rospy.Rate(rospy.get_param("config/rate")) + self.declare_parameter('dummy.heading', 0.0) + self.declare_parameter('config.rate', 10) - while not rospy.is_shutdown(): - heading_pub.publish(currentHeading) - rate.sleep() + self.heading = self.get_parameter('dummy.heading').value + rate_hz = self.get_parameter('config.rate').value + self.heading_pub = self.create_publisher(Float32, 'heading', 10) + self.timer = self.create_timer(1.0 / rate_hz, self.publish_heading) -if __name__ == '__main__': + def publish_heading(self): + msg = Float32() + msg.data = float(self.heading) + self.heading_pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = DummyHeading() try: - heading_pub = rospy.Publisher('heading', Float32, queue_size=10) - rospy.init_node("publish_dummy_heading_data", anonymous=True) - heading_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/dummy_position b/src/sailing_robot/scripts/dummy_position index 0cd36512..f7114959 100755 --- a/src/sailing_robot/scripts/dummy_position +++ b/src/sailing_robot/scripts/dummy_position @@ -1,45 +1,55 @@ -#!/usr/bin/python -# -# Node publishes apparent wind direction -# DUMMY NODE: keeps publishing a constant wind direction as specified in the -# parameters file. See 'gps' for our real gps node. +#!/usr/bin/env python3 from datetime import datetime -import rospy - +import rclpy +from rclpy.node import Node from sensor_msgs.msg import NavSatFix from sailing_robot.msg import gpswtime -currentLatitude = rospy.get_param("dummy/latitude") -currentLongitude = rospy.get_param("dummy/longitude") -position = NavSatFix() +class DummyPosition(Node): + def __init__(self): + super().__init__('publish_dummy_position_data') + + self.declare_parameter('dummy.latitude', 50.8) + self.declare_parameter('dummy.longitude', 1.0) + self.declare_parameter('config.rate', 10) + + self.latitude = self.get_parameter('dummy.latitude').value + self.longitude = self.get_parameter('dummy.longitude').value + rate_hz = self.get_parameter('config.rate').value -def position_publisher(): - rate = rospy.Rate(rospy.get_param("config/rate")) + self.position_pub = self.create_publisher(NavSatFix, 'position', 10) + self.gps_pub = self.create_publisher(gpswtime, 'gps_fix', 10) - position.latitude = currentLatitude - position.longitude = currentLongitude + self.timer = self.create_timer(1.0 / rate_hz, self.publish_position) + + def publish_position(self): + position = NavSatFix() + position.latitude = float(self.latitude) + position.longitude = float(self.longitude) + self.position_pub.publish(position) - while not rospy.is_shutdown(): - position_pub.publish(position) - now = datetime.utcnow() wtime = gpswtime() wtime.fix = position wtime.time_h = now.hour wtime.time_m = now.minute wtime.time_s = now.second - gps_pub.publish(wtime) - rate.sleep() + self.gps_pub.publish(wtime) -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = DummyPosition() try: - position_pub = rospy.Publisher('position', NavSatFix, queue_size=10) - gps_pub = rospy.Publisher('gps_fix', gpswtime, queue_size=10) - rospy.init_node("publish_dummy_position_data", anonymous=True) - - position_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/force_jibe_tack b/src/sailing_robot/scripts/force_jibe_tack index 2055ebe1..f89a4817 100755 --- a/src/sailing_robot/scripts/force_jibe_tack +++ b/src/sailing_robot/scripts/force_jibe_tack @@ -1,65 +1,61 @@ -#!/usr/bin/python +#!/usr/bin/env python3 -import rospy -from std_msgs.msg import Float32, Float64, String +import time +import rclpy +from rclpy.node import Node +from std_msgs.msg import String -import time, math - -class Force_jibe(): +class ForceJibe(Node): def __init__(self): - """ - This node detect if a tack or a jibe fails (taking too much time) - and triggers a jibe/tack - """ - self.jibe_tack_now_pub = rospy.Publisher('jibe_tack_now', String, queue_size=10) + super().__init__('Force_jibe') + + self.declare_parameter('config.rate', 10) + self.declare_parameter('force_jibe.time_tack', 30.0) - rospy.init_node("Force_jibe", anonymous=True) + rate_hz = self.get_parameter('config.rate').value + self.time_tack = self.get_parameter('force_jibe.time_tack').value - rospy.Subscriber('sailing_state', String, self.update_sailing_state) self.sailing_state = 'normal' self.previous_sailing_state = 'normal' - self.timer = time.time() + self.timer_start = time.time() + self.jibe_tack_now_pub = self.create_publisher(String, 'jibe_tack_now', 10) + self.create_subscription(String, 'sailing_state', self.update_sailing_state, 10) - self.rate = rospy.Rate(rospy.get_param("config/rate")) - - self.time_tack = rospy.get_param("force_jibe/time_tack") - self.looper() + self.timer = self.create_timer(1.0 / rate_hz, self.looper) def update_sailing_state(self, msg): self.sailing_state = msg.data - - # reset timer when the sailing state changes - if msg.data == 'normal' or \ - self.previous_sailing_state != self.sailing_state: - self.timer = time.time() - self.previous_sailing_state = self.sailing_state + if msg.data == 'normal' or self.previous_sailing_state != self.sailing_state: + self.timer_start = time.time() + self.previous_sailing_state = self.sailing_state def looper(self): - - while not rospy.is_shutdown(): - - # check tacking issue based on its duration - if self.sailing_state != 'normal' and \ - (time.time() - self.timer) > self.time_tack: - rospy.logerr("Issue with tacking/jibing") - - # "auto" is set, hence the boat will tack if it was jibing and jibe if it was tacking - self.jibe_tack_now_pub.publish('auto') - - # wait untill the sailing state comes back to normal - while self.sailing_state != 'normal' and not rospy.is_shutdown(): - self.rate.sleep() - self.timer = time.time() - - - self.rate.sleep() - - -if __name__ == '__main__': + if self.sailing_state != 'normal': + elapsed = time.time() - self.timer_start + if elapsed > self.time_tack: + self.get_logger().warn('Forcing jibe/tack after %.1fs' % elapsed) + msg = String() + if self.sailing_state == 'switch_to_port_tack': + msg.data = 'jibe_now_port' + else: + msg.data = 'jibe_now_stbd' + self.jibe_tack_now_pub.publish(msg) + self.timer_start = time.time() + + +def main(args=None): + rclpy.init(args=args) + node = ForceJibe() try: - Force_jibe() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/helming b/src/sailing_robot/scripts/helming index 5672269f..0b62de37 100755 --- a/src/sailing_robot/scripts/helming +++ b/src/sailing_robot/scripts/helming @@ -1,327 +1,277 @@ -#!/usr/bin/python +#!/usr/bin/env python3 -import rospy import time import collections import numpy as np import random -from std_msgs.msg import Float64, Float32, Int16, String -from std_msgs.msg import Bool +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64, Float32, Int16, String, Bool from sailing_robot.sail_table import SailTable, SailData - from sailing_robot.pid_data import PID_Data import sailing_robot.pid_control as _PID from sailing_robot.navigation import angle_subtract -# Sheet control WIND = object() SHEET_IN = object() SHEET_OUT = object() -# Rudder control PID_GOAL_HEADING = object() -PID_ANGLE_TO_WIND = object() # set an angle to the wind, 0 being going torward the wind, can be either 0/360 or -180/180 -RUDDER_FULL_LEFT = object() # the boat is going to the left -RUDDER_FULL_RIGHT = object() # the boat is going to the right - -# Publishers for rudder and sailsheet control -PUB_RUDDER = rospy.Publisher('rudder_control', Int16, queue_size=10) # Use UInt 16 here to minimize the memory use -PUB_SAILSHEET = rospy.Publisher('sailsheet_normalized', Float32, queue_size=10) - -PUB_dbg_helming = rospy.Publisher('dbg_helming_procedure', String, queue_size=10) - - -data = PID_Data() -rudder = rospy.get_param('rudder') -controller = _PID.PID(rudder['control']['Kp'], rudder['control']['Ki'], rudder['control']['Kd'],rudder['maxAngle'], -rudder['maxAngle']) -remote_control = False - -sail_table_dict = rospy.get_param('sailsettings/table') -sheet_out_to_jibe = rospy.get_param('sailsettings/sheet_out_to_jibe', False) -sail_table = SailTable(sail_table_dict) -sail_data = SailData(sail_table) - -TIMEOUT = rospy.get_param('procedure/timeout') -EXPLORE_COEF = rospy.get_param('procedure/exploration_coefficient') - -########################################################################## -def update_remote_control(msg): - remote_control = msg.data - -def set_sail(sheet_control, offset=0.0): - if sheet_control is WIND: - sheet_normalized = sail_data.calculate_sheet_setting() + offset - - - elif sheet_control is IN: - sheet_normalized = 0 - - elif sheet_control is OUT: - sheet_normalized = 1 - - elif sheet_control is float: - sheet_normalized = sheet_control - - # be sure we don't publish values above 1 and below 0 - sheet_normalized = np.clip(sheet_normalized, 0, 1) - PUB_SAILSHEET.publish(sheet_normalized) - - -def set_rudder(state, angle_to_wind=0): - if state is PID_GOAL_HEADING: - rawangle = -controller.update_PID(angle_subtract(data.heading, data.goal_heading)) - angle = _PID.saturation(rawangle,-rudder['maxAngle'], rudder['maxAngle']) - - elif state is PID_ANGLE_TO_WIND: - rawangle = -controller.update_PID(angle_subtract(angle_to_wind, sail_data.wind_direction_apparent)) - angle = _PID.saturation(rawangle,-rudder['maxAngle'], rudder['maxAngle']) +PID_ANGLE_TO_WIND = object() +RUDDER_FULL_LEFT = object() +RUDDER_FULL_RIGHT = object() - elif state is RUDDER_FULL_LEFT: - angle = rudder['maxAngle'] - elif state is RUDDER_FULL_RIGHT: - angle = -rudder['maxAngle'] +class Helming(Node): + def __init__(self): + super().__init__('helming') + + self.declare_parameter('rudder.control.Kp', 0.5) + self.declare_parameter('rudder.control.Ki', 0.0) + self.declare_parameter('rudder.control.Kd', 0.0) + self.declare_parameter('rudder.maxAngle', 40.0) + self.declare_parameter('sailsettings.table', '{}') + self.declare_parameter('sailsettings.sheet_out_to_jibe', False) + self.declare_parameter('procedure.timeout', 30.0) + self.declare_parameter('procedure.exploration_coefficient', 0.1) + self.declare_parameter('procedure.jibe_to_turn', False) + self.declare_parameter('config.rate', 10) + + kp = self.get_parameter('rudder.control.Kp').value + ki = self.get_parameter('rudder.control.Ki').value + kd = self.get_parameter('rudder.control.Kd').value + self.max_angle = self.get_parameter('rudder.maxAngle').value + sail_table_val = self.get_parameter('sailsettings.table').value + if isinstance(sail_table_val, str): + import yaml + sail_table_dict = yaml.safe_load(sail_table_val) or {} + else: + sail_table_dict = sail_table_val or {} + self.timeout = self.get_parameter('procedure.timeout').value + self.explore_coef = self.get_parameter('procedure.exploration_coefficient').value + jibe_to_turn = self.get_parameter('procedure.jibe_to_turn').value + rate_hz = self.get_parameter('config.rate').value + + self.controller = _PID.PID(kp, ki, kd, self.max_angle, -self.max_angle) + sail_table = SailTable(sail_table_dict) + self.sail_data = SailData(sail_table) + self.data = PID_Data() + self.remote_control = False + + self.pub_rudder = self.create_publisher(Int16, 'rudder_control', 10) + self.pub_sailsheet = self.create_publisher(Float32, 'sailsheet_normalized', 10) + self.pub_dbg = self.create_publisher(String, 'dbg_helming_procedure', 10) + + self.create_subscription(Float64, 'wind_direction_apparent', self.sail_data.update_wind, 10) + self.create_subscription(Float32, 'goal_heading', self.data.update_goal_heading, 10) + self.create_subscription(Float32, 'heading', self.data.update_heading, 10) + self.create_subscription(String, 'sailing_state', self.data.update_sailing_state, 10) + self.create_subscription(Float32, 'tack_rudder', self.data.update_tack_rudder, 10) + self.create_subscription(Bool, 'remote_control', self._update_remote_control, 10) + + if jibe_to_turn: + procedure_list = [JibeBasic, TackBasic, TackSheetOut, TackIncreaseAngleToWind] + else: + procedure_list = [TackBasic, TackSheetOut, TackIncreaseAngleToWind, JibeBasic] - PUB_RUDDER.publish(int(angle)) + self.proc = ProcedureHandle(procedure_list, self.timeout, self.explore_coef) + self.timer = self.create_timer(1.0 / rate_hz, self.run_loop) -########################################################################## + def _update_remote_control(self, msg): + self.remote_control = msg.data + def set_sail(self, sheet_control, offset=0.0): + if sheet_control is WIND: + sheet_normalized = self.sail_data.calculate_sheet_setting() + offset + elif sheet_control is SHEET_IN: + sheet_normalized = 0.0 + elif sheet_control is SHEET_OUT: + sheet_normalized = 1.0 + else: + sheet_normalized = float(sheet_control) + sheet_normalized = float(np.clip(sheet_normalized, 0, 1)) + msg = Float32() + msg.data = sheet_normalized + self.pub_sailsheet.publish(msg) + + def set_rudder(self, state, angle_to_wind=0): + if state is PID_GOAL_HEADING: + rawangle = -self.controller.update_PID(angle_subtract(self.data.heading, self.data.goal_heading)) + angle = _PID.saturation(rawangle, -self.max_angle, self.max_angle) + elif state is PID_ANGLE_TO_WIND: + rawangle = -self.controller.update_PID(angle_subtract(angle_to_wind, self.sail_data.wind_direction_apparent)) + angle = _PID.saturation(rawangle, -self.max_angle, self.max_angle) + elif state is RUDDER_FULL_LEFT: + angle = self.max_angle + elif state is RUDDER_FULL_RIGHT: + angle = -self.max_angle + else: + angle = 0.0 + msg = Int16() + msg.data = int(angle) + self.pub_rudder.publish(msg) + + def publish_dbg(self, text): + msg = String() + msg.data = text + self.pub_dbg.publish(msg) + + def run_loop(self): + if self.data.sailing_state == 'normal': + if self.proc.procedure_in_progress(): + self.get_logger().warn('Procedure success %s in %.2fs' % ( + str(self.proc.current_procedure), + self.proc.current_procedure.elapsed_time())) + self.proc.mark_success(self.remote_control, self.publish_dbg) + self.set_rudder(PID_GOAL_HEADING) + self.set_sail(WIND) + else: + self.run_procedure() + + def run_procedure(self): + if (not self.proc.procedure_in_progress()) or (self.data.sailing_state != self.proc.current_procedure.sailing_state): + self.proc.first_procedure() + self.proc.start_procedure(self.data.sailing_state) + self.get_logger().warn('Run procedure ' + str(self.proc.current_procedure)) + self.publish_dbg('start ' + str(self.proc.current_procedure)) + elif self.proc.current_procedure.has_failed(): + self.get_logger().warn('Procedure failed ' + str(self.proc.current_procedure)) + self.proc.mark_failure(self.remote_control, self.publish_dbg) + self.proc.next_procedure() + self.proc.start_procedure(self.data.sailing_state) + self.get_logger().warn('Run procedure ' + str(self.proc.current_procedure)) + self.publish_dbg('start ' + str(self.proc.current_procedure)) + + sail, rudder = self.proc.current_procedure.loop() + self.set_sail(sail) + if isinstance(rudder, tuple): + self.set_rudder(rudder[0], angle_to_wind=rudder[1]) + else: + self.set_rudder(rudder) -class ProcedureBase(object): - def __init__(self, sailing_state, timeout=TIMEOUT): +class ProcedureBase: + def __init__(self, sailing_state, timeout): self.start_time = time.time() self.timeout = timeout self.sailing_state = sailing_state def has_failed(self): - """ - Am I out of time? - Am I failing? - """ - currenttime = time.time() - return self.EnlapsedTime() > self.timeout + return self.elapsed_time() > self.timeout - def EnlapsedTime(self): - currenttime = time.time() - return currenttime - self.start_time + def elapsed_time(self): + return time.time() - self.start_time def __str__(self): return self.__class__.__name__ class TackBasic(ProcedureBase): - """ - Basic Tack procedure - """ - def __init__(self, sailing_state, timeout=TIMEOUT): - super(TackBasic, self).__init__(sailing_state, timeout) - def loop(self): - set_sail(WIND) - if self.sailing_state == "switch_to_port_tack": - set_rudder(RUDDER_FULL_RIGHT) + if self.sailing_state == 'switch_to_port_tack': + return WIND, RUDDER_FULL_RIGHT else: - set_rudder(RUDDER_FULL_LEFT) - + return WIND, RUDDER_FULL_LEFT class JibeBasic(ProcedureBase): - """ - Basic Jibe procedure - """ - def __init__(self, sailing_state, timeout=TIMEOUT): - super(JibeBasic, self).__init__(sailing_state, timeout) - def loop(self): - # sheet out a bit more than what is given by the look up table - set_sail(WIND, offset=+0.2) - if self.sailing_state == "switch_to_port_tack": - set_rudder(RUDDER_FULL_LEFT) + if self.sailing_state == 'switch_to_port_tack': + return (WIND, 0.2), RUDDER_FULL_LEFT else: - set_rudder(RUDDER_FULL_RIGHT) + return (WIND, 0.2), RUDDER_FULL_RIGHT class TackSheetOut(ProcedureBase): - """ - Tack procedure where we sheet out a bit - - When sheeted in completely the jib has too much power and tacking becomes impossible - in some strong conditions. Hence sheeting out is needed, however if the sails are out - too much the boat will not have enough power to tack - """ - def __init__(self, sailing_state, timeout=TIMEOUT): - super(TackSheetOut, self).__init__(sailing_state, timeout) - def loop(self): - # sheet out a bit more than what is given by the look up table - set_sail(WIND, offset=+0.2) - if self.sailing_state == "switch_to_port_tack": - set_rudder(RUDDER_FULL_RIGHT) + if self.sailing_state == 'switch_to_port_tack': + return (WIND, 0.2), RUDDER_FULL_RIGHT else: - set_rudder(RUDDER_FULL_LEFT) + return (WIND, 0.2), RUDDER_FULL_LEFT - -class Tack_IncreaseAngleToWind(ProcedureBase): - """ - More advance Tack procedure, building speed for 5s by going less upwind - """ - def __init__(self, sailing_state, timeout=TIMEOUT): - super(Tack_IncreaseAngleToWind, self).__init__(sailing_state, timeout) +class TackIncreaseAngleToWind(ProcedureBase): + def __init__(self, sailing_state, timeout): + super().__init__(sailing_state, timeout) self.beating_angle = 80 def loop(self): - set_sail(WIND) - if self.EnlapsedTime() < 4: - if self.sailing_state == "switch_to_port_tack": - set_rudder(PID_ANGLE_TO_WIND, angle_to_wind = self.beating_angle) + if self.elapsed_time() < 4: + if self.sailing_state == 'switch_to_port_tack': + return WIND, (PID_ANGLE_TO_WIND, self.beating_angle) else: - set_rudder(PID_ANGLE_TO_WIND, angle_to_wind = 360-self.beating_angle) + return WIND, (PID_ANGLE_TO_WIND, 360 - self.beating_angle) else: - if self.sailing_state == "switch_to_port_tack": - set_rudder(RUDDER_FULL_RIGHT) + if self.sailing_state == 'switch_to_port_tack': + return WIND, RUDDER_FULL_RIGHT else: - set_rudder(RUDDER_FULL_LEFT) - - + return WIND, RUDDER_FULL_LEFT -########################################################################## -class ProcedureHandle(): - """ - Class to handle a list of procedure and the priority based on weights - after each tack attempt weight based on time taken by the procedure - are given to each procedure for the future. - """ - def __init__(self, ProcedureList): - - # Initialisation of the procedure list with initial weights, integers with increment of 1 - self.ProcedureList = [ {"Procedure": Procedure, - "TimeList": collections.deque(maxlen = 10), - "InitPos": i} for i,Procedure in enumerate(ProcedureList) ] - - self.currentProcedureId = 0 - self.currentProcedure = None +class ProcedureHandle: + def __init__(self, procedure_list, timeout, explore_coef): + self.timeout = timeout + self.explore_coef = explore_coef + self.procedure_list = [ + {'Procedure': p, 'TimeList': collections.deque(maxlen=10), 'InitPos': i} + for i, p in enumerate(procedure_list) + ] + self.current_procedure_id = 0 + self.current_procedure = None - def ProcedureInProgress(self): - return (self.currentProcedure != None) + def procedure_in_progress(self): + return self.current_procedure is not None - def FirstProcedure(self): - self.OrderList() - self.currentProcedureId = 0 + def first_procedure(self): + self._order_list() + self.current_procedure_id = 0 - def NextProcedure(self): - self.currentProcedureId = (self.currentProcedureId + 1) % len(self.ProcedureList) + def next_procedure(self): + self.current_procedure_id = (self.current_procedure_id + 1) % len(self.procedure_list) - def OrderList(self): + def _order_list(self): def get_weight(x): if x['TimeList']: return np.mean(x['TimeList']) else: - if random.random() < (EXPLORE_COEF / sum([ not x['TimeList'] for x in self.ProcedureList])): - rospy.logwarn("Random procedure picked") - # Add some randomness in choice for untested procedures - return 0.1*random.random() + untested = sum(not p['TimeList'] for p in self.procedure_list) + if random.random() < (self.explore_coef / untested): + return 0.1 * random.random() else: - # Just to keep the order given at first if the procedure was not tested yet - return TIMEOUT + x['InitPos']*0.01*TIMEOUT - - self.ProcedureList = sorted(self.ProcedureList, key=get_weight) - # rospy.logwarn(str(self.ProcedureList)) + return self.timeout + x['InitPos'] * 0.01 * self.timeout + self.procedure_list = sorted(self.procedure_list, key=get_weight) - def MarkSuccess(self): + def mark_success(self, remote_control, publish_dbg): if not remote_control: - PUB_dbg_helming.publish("success " + str(self.currentProcedure)) - self.ProcedureList[self.currentProcedureId]['TimeList'].append(self.currentProcedure.EnlapsedTime()) - self.currentProcedure = None + publish_dbg('success ' + str(self.current_procedure)) + self.procedure_list[self.current_procedure_id]['TimeList'].append( + self.current_procedure.elapsed_time()) + self.current_procedure = None - def MarkFailure(self): + def mark_failure(self, remote_control, publish_dbg): if not remote_control: - PUB_dbg_helming.publish("fail " + str(self.currentProcedure)) - # In case of failure the weight given is 1.5 times the timeout - self.ProcedureList[self.currentProcedureId]['TimeList'].append(1.5*TIMEOUT) - self.currentProcedure = None + publish_dbg('fail ' + str(self.current_procedure)) + self.procedure_list[self.current_procedure_id]['TimeList'].append(1.5 * self.timeout) + self.current_procedure = None - def StartProcedure(self, sailing_state): - self.currentProcedure = self.ProcedureList[self.currentProcedureId]['Procedure'](sailing_state) - PUB_dbg_helming.publish("start " + str(self.currentProcedure)) + def start_procedure(self, sailing_state): + self.current_procedure = self.procedure_list[self.current_procedure_id]['Procedure']( + sailing_state, self.timeout) -########################################################################## - - - -class Helming(): - def __init__(self): - rospy.init_node('helming', anonymous=True) - self.rate = rospy.Rate(rospy.get_param("config/rate")) - - # Initialisation of the procedure list - if rospy.get_param('procedure/jibe_to_turn'): - procedureList = [JibeBasic, TackBasic, TackSheetOut, Tack_IncreaseAngleToWind] - else: - procedureList = [TackBasic, TackSheetOut, Tack_IncreaseAngleToWind, JibeBasic] - - - self.Proc = ProcedureHandle(procedureList) - self.Runner() - - def Runner(self): - while not rospy.is_shutdown(): - if data.sailing_state == 'normal': - if self.Proc.ProcedureInProgress(): - # ending a procedure because it is finished according to the highlevel - rospy.logwarn("Procedure success "+ str(self.Proc.currentProcedure)+ - " in "+ '{:.2f}'.format(self.Proc.currentProcedure.EnlapsedTime()) + "s") - self.Proc.MarkSuccess() - - set_rudder(PID_GOAL_HEADING) - set_sail(WIND) - else: - # Continuing a procedure - self.runProcedure() - - self.rate.sleep() - - - def runProcedure(self): - if (not self.Proc.ProcedureInProgress()) or (data.sailing_state != self.Proc.currentProcedure.sailing_state): - # no procedure have been started (=we just decided to swich tack) - self.Proc.FirstProcedure() - self.Proc.StartProcedure(data.sailing_state) - rospy.logwarn("Run procedure " + str(self.Proc.currentProcedure)) - - elif self.Proc.currentProcedure.has_failed(): - rospy.logwarn("Procedure failed " + str(self.Proc.currentProcedure)) - - self.Proc.MarkFailure() - # if time out we start the next procedure in the list - self.Proc.NextProcedure() - self.Proc.StartProcedure(data.sailing_state) - rospy.logwarn("Run procedure " + str(self.Proc.currentProcedure)) - - # we advance to the next timestep - self.Proc.currentProcedure.loop() - - - - -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = Helming() try: - rospy.Subscriber('wind_direction_apparent', Float64, sail_data.update_wind) - rospy.Subscriber('goal_heading', Float32, data.update_goal_heading) - rospy.Subscriber('heading', Float32, data.update_heading) - rospy.Subscriber('sailing_state', String, data.update_sailing_state) - rospy.Subscriber('tack_rudder', Float32, data.update_tack_rudder) - rospy.Subscriber('remote_control', Bool, update_remote_control) - Helming() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass - -######################################################################## - + finally: + node.destroy_node() + rclpy.shutdown() +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_camera_detect b/src/sailing_robot/scripts/sensor_camera_detect index cd4a1c29..18e6228e 100755 --- a/src/sailing_robot/scripts/sensor_camera_detect +++ b/src/sailing_robot/scripts/sensor_camera_detect @@ -1,105 +1,107 @@ -#!/usr/bin/python -# READY FOR MIT - -from std_msgs.msg import String +#!/usr/bin/env python3 +"""Camera-based obstacle detection node.""" import collections -import rospy -import time - -import numpy as np -import cv2 +import rclpy +from rclpy.node import Node +from std_msgs.msg import String import datetime - import os +try: + import numpy as np + import cv2 + CV2_AVAILABLE = True +except ImportError: + CV2_AVAILABLE = False -class Camera_detection(): - """ - Node that publishes the string 'detected' every time an obstacle is detected; - otherwise, the string 'nothing' is published. - - An obstacle is detected when more than half of the images within the given averaging time - have a detection area larger than the threshold. - The detection area is the fraction of the total frame that is covered - by a colour within the given colour range. - both the color range and the minimum needed area for obstacle detection come from ROS param. - All photos taken for the obstacle detection are saved in ~/camera_detect_obstacle_$datetime - """ +class CameraDetection(Node): def __init__(self): - - self.publisher = rospy.Publisher('camera_detection', String, queue_size=10) - - rospy.init_node("sensor_camera_detect", anonymous=True) - - sensor_rate = rospy.get_param("camera_detection/rate") - self.rate = rospy.Rate(sensor_rate) - - self.Lower_hsv1 = np.array( rospy.get_param('camera_detection/Lower_color_hsv1')) - self.Upper_hsv1 = np.array( rospy.get_param('camera_detection/Upper_color_hsv1')) - self.Lower_hsv2 = np.array( rospy.get_param('camera_detection/Lower_color_hsv2')) - self.Upper_hsv2 = np.array( rospy.get_param('camera_detection/Upper_color_hsv2')) - - self.threshold = rospy.get_param('camera_detection/threshold') - AVE_TIME = rospy.get_param("camera_detection/average_time") # lengh of the averaging in seconds - self.AVE_SIZE = int(AVE_TIME * sensor_rate) # size of the averaging sample - self.average_list = collections.deque(maxlen = self.AVE_SIZE) - self.empty_image_counter = 0 # only save every empty_image_ignore-th empty image + super().__init__('sensor_camera_detect') + + self.declare_parameter('camera_detection.rate', 5.0) + self.declare_parameter('camera_detection.threshold', 0.1) + self.declare_parameter('camera_detection.average_time', 2.0) + self.declare_parameter('camera_detection.Lower_color_hsv1', [0, 100, 100]) + self.declare_parameter('camera_detection.Upper_color_hsv1', [10, 255, 255]) + self.declare_parameter('camera_detection.Lower_color_hsv2', [160, 100, 100]) + self.declare_parameter('camera_detection.Upper_color_hsv2', [180, 255, 255]) + + sensor_rate = self.get_parameter('camera_detection.rate').value + self.threshold = self.get_parameter('camera_detection.threshold').value + ave_time = self.get_parameter('camera_detection.average_time').value + self.ave_size = int(ave_time * sensor_rate) + self.average_list = collections.deque(maxlen=self.ave_size) + + if CV2_AVAILABLE: + self.lower_hsv1 = np.array(self.get_parameter('camera_detection.Lower_color_hsv1').value) + self.upper_hsv1 = np.array(self.get_parameter('camera_detection.Upper_color_hsv1').value) + self.lower_hsv2 = np.array(self.get_parameter('camera_detection.Lower_color_hsv2').value) + self.upper_hsv2 = np.array(self.get_parameter('camera_detection.Upper_color_hsv2').value) + else: + self.get_logger().warn('OpenCV not available - camera detection disabled') + + self.publisher = self.create_publisher(String, 'camera_detection', 10) + self.timer = self.create_timer(1.0 / sensor_rate, self.publish) + self.camera = None + self.image = None + self.empty_image_counter = 0 self.empty_image_ignore = 5 - self.publish() def publish(self): - - camera = cv2.VideoCapture(0) - # recording 50 images at start to wait for the white balancing to be operational - for i in range(50): - (bool, image) = camera.read() - - rospy.logwarn('Camera ready') - image_size = image.shape[0]*image.shape[1] - self.rate.sleep() - - while not rospy.is_shutdown(): - (bool, image) = camera.read() - if bool: - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - mask_hsv1 = cv2.inRange(hsv, self.Lower_hsv1, self.Upper_hsv1) - percent_detect1 = 1.0*cv2.countNonZero(mask_hsv1) / image_size # percentage of the image that contains the expected colors - - # if only 1 set of color is used, we compute only 1 mask - if self.Lower_hsv2[0] != 0 or self.Upper_color_hsv2[0] != 0: - mask_hsv2 = cv2.inRange(hsv, self.Lower_hsv2, self.Upper_hsv2) - percent_detect2 = 1.0*cv2.countNonZero(mask_hsv2) / image_size # percentage of the image that contains the expected colors - else: - percent_detect2 = 0 - - if percent_detect1+percent_detect2 >= self.threshold: - self.average_list.append(1) - else: - self.average_list.append(0) - - if 1.0*sum(self.average_list)/self.AVE_SIZE > 0.5: - msg = 'detected' - filename = os.path.expanduser('~/camera_detected_obstacle_{:%Y-%m-%d_%H:%M:%S_%f}.jpg'.format(datetime.datetime.now())) - cv2.imwrite(filename, image) - else: - msg = 'nothing' - self.empty_image_counter +=1 - if self.empty_image_counter > self.empty_image_ignore: - filename = os.path.expanduser('~/camera_detected_obstacle_{:%Y-%m-%d_%H:%M:%S_%f}_nothing.jpg'.format(datetime.datetime.now())) - cv2.imwrite(filename, image) - self.empty_image_counter = 0 - - - self.publisher.publish(msg) - - self.rate.sleep() - - -if __name__ == '__main__': + if not CV2_AVAILABLE: + msg = String() + msg.data = 'nothing' + self.publisher.publish(msg) + return + + if self.camera is None: + self.camera = cv2.VideoCapture(0) + self.get_logger().info('Camera ready') + + ret, image = self.camera.read() + if not ret or image is None: + return + + self.image = image + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + image_size = image.shape[0] * image.shape[1] + + mask1 = cv2.inRange(hsv, self.lower_hsv1, self.upper_hsv1) + mask2 = cv2.inRange(hsv, self.lower_hsv2, self.upper_hsv2) + mask = cv2.bitwise_or(mask1, mask2) + detection_area = float(np.sum(mask > 0)) / image_size + + self.average_list.append(detection_area > self.threshold) + + if len(self.average_list) >= self.ave_size: + detected_fraction = sum(self.average_list) / len(self.average_list) + result = 'detected' if detected_fraction > 0.5 else 'nothing' + else: + result = 'nothing' + + msg = String() + msg.data = result + self.publisher.publish(msg) + + def destroy_node(self): + if self.camera is not None: + self.camera.release() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = CameraDetection() try: - Camera_detection() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_driver_battery b/src/sailing_robot/scripts/sensor_driver_battery index eeaa56bd..252a658f 100755 --- a/src/sailing_robot/scripts/sensor_driver_battery +++ b/src/sailing_robot/scripts/sensor_driver_battery @@ -1,49 +1,59 @@ -#!/usr/bin/python - +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node from sailing_robot.msg import BatteryState -from ina219 import INA219 -import rospy - +try: + from ina219 import INA219 + INA_AVAILABLE = True +except ImportError: + INA_AVAILABLE = False -class Battery(): - """ - Node that publishes current and voltage from the ina219 module - """ +class Battery(Node): def __init__(self): - # initialize the library - SHUNT_OHMS = 0.1 - MAX_EXPECTED_AMPS = 1 - self.ina = INA219(SHUNT_OHMS, MAX_EXPECTED_AMPS) - self.ina.configure(self.ina.RANGE_16V, self.ina.GAIN_AUTO) - self.ina.sleep() + super().__init__('battery') + self.declare_parameter('config.battery_rate', 1.0) + rate_hz = self.get_parameter('config.battery_rate').value - self.battery_pub = rospy.Publisher('battery', BatteryState, queue_size=10) - rospy.init_node("battery", anonymous=True) - - self.rate = rospy.Rate(rospy.get_param("config/battery_rate")) - self.battery_publisher() + self.battery_pub = self.create_publisher(BatteryState, 'battery', 10) + if INA_AVAILABLE: + shunt_ohms = 0.1 + max_expected_amps = 1 + self.ina = INA219(shunt_ohms, max_expected_amps) + self.ina.configure(self.ina.RANGE_16V, self.ina.GAIN_AUTO) + self.ina.sleep() + else: + self.ina = None + self.get_logger().warn('ina219 not available - running in simulation mode') - def battery_publisher(self): + self.timer = self.create_timer(1.0 / rate_hz, self.publish_battery) - while not rospy.is_shutdown(): - msg = BatteryState() + def publish_battery(self): + msg = BatteryState() + if self.ina: self.ina.wake() - msg.voltage = self.ina.voltage() + msg.voltage = self.ina.voltage() msg.current = -self.ina.current() msg.power = -msg.voltage * msg.current self.ina.sleep() - self.battery_pub.publish(msg) - self.rate.sleep() + self.battery_pub.publish(msg) -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = Battery() try: - Battery() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_driver_gps b/src/sailing_robot/scripts/sensor_driver_gps index d534f37a..e49a31b9 100755 --- a/src/sailing_robot/scripts/sensor_driver_gps +++ b/src/sailing_robot/scripts/sensor_driver_gps @@ -1,47 +1,43 @@ -#!/usr/bin/python -"""Read from the GPS receiver. - -Publishes: -- position (NavSatFix) -- gps_fix (gpswtime) - includes timestamp from GPS signal -- gps_satellites (Int16) - number of satellites visible -""" +#!/usr/bin/env python3 +"""Read from the GPS receiver.""" from datetime import datetime import os.path -import serial -import pynmea2 import re import time import traceback -import rospy -import smbus +import rclpy +from rclpy.node import Node from std_msgs.msg import Int16 from sensor_msgs.msg import NavSatFix from sailing_robot.msg import gpswtime, Velocity from sailing_robot.gps_utils import UBXMessage, get_port, UbxNmeaParser -BAUD_RATE = 9600 -READ_TIMEOUT = 0.5 -FILENAME_BASE = "~/sailing-robot/gps-raw-nmea_{}_{}" +try: + import serial + SERIAL_AVAILABLE = True +except ImportError: + SERIAL_AVAILABLE = False -i2c_ADDRESS = 0x42 +try: + import smbus + SMBUS_AVAILABLE = True +except ImportError: + SMBUS_AVAILABLE = False -params = rospy.get_param('/') +try: + import pynmea2 + PYNMEA2_AVAILABLE = True +except ImportError: + PYNMEA2_AVAILABLE = False -if params.get('log_raw_gps', True): - filename = FILENAME_BASE.format(params.get('log_name', ''), - datetime.now().strftime('%Y-%m-%dT%H.%M.%S')) - filename = os.path.expanduser(filename) - raw_log = open(filename, 'wb') -else: - raw_log = None +BAUD_RATE = 9600 +READ_TIMEOUT = 0.5 +FILENAME_BASE = '~/sailing-robot/gps-raw-nmea_{}_{}' +I2C_ADDRESS = 0x42 -def decimal_degrees(d_m, hemisphere): - """Convert the degrees & minutes number from the GPS to decimal degrees - We get the degrees and minutes unseparated, i.e. (100*degrees)+minutes - """ +def decimal_degrees(d_m, hemisphere): m = re.match(r'(\d+)(\d{2}\.\d+)', str(d_m)) if not m: raise ValueError(d_m) @@ -51,130 +47,162 @@ def decimal_degrees(d_m, hemisphere): return -res return res -def pos_publisher(): - use_i2c = rospy.get_param("gps_via_i2c") - if use_i2c: - i2c_bus = smbus.SMBus(1) - set_gps_options(i2c_bus, use_i2c) - else: - serial_port = serial.Serial(get_port(), BAUD_RATE, timeout=READ_TIMEOUT) - set_gps_options(serial_port, use_i2c) - - gps_reader = UbxNmeaParser() - - while not rospy.is_shutdown(): - if use_i2c: - data = b'' - for _ in range(64): - char = chr(i2c_bus.read_byte(i2c_ADDRESS)) - if char != b'\xff': - data += char - else: - data = serial_port.read(64) - if raw_log: - raw_log.write(data) - raw_log.flush() +class SensorDriverGps(Node): + def __init__(self): + super().__init__('sensor_driver_gps') - gps_reader.feed(data) + self.declare_parameter('gps_via_i2c', False) + self.declare_parameter('log_raw_gps', True) + self.declare_parameter('log_name', '') + self.declare_parameter('change_gps_rate', False) - try: - batch = list(gps_reader.get_msgs()) - except (pynmea2.ParseError, pynmea2.ChecksumError, UnicodeError): - s = "Error parsing GPS data.\nbuffer={!r}\ndata={!r}\n{}".format( - gps_reader.buf, data, traceback.format_exc() - ) - rospy.logwarn(s) - gps_reader.buf = b'' - continue - - if len(gps_reader.buf) > 512: - # I don't really understand this, but sometimes the data is - # gibberish which makes no sense. If it seems like this is happening, - # shut it down and reopen it. 512 bytes is hopefully larger than any - # message we want. - rospy.logwarn("512 bytes unprocessed in GPS serial buffer - resetting serial port") - if not use_i2c: - serial_port.close() - serial_port = serial.Serial(get_port(), BAUD_RATE, timeout=READ_TIMEOUT) - gps_reader = UbxNmeaParser() - - for sentence in batch: - rospy.logdebug("GPS received {!r}".format(sentence)) - if not isinstance(sentence, pynmea2.NMEASentence): - continue - if sentence.sentence_type == 'VTG': - velocity = Velocity() - if sentence.spd_over_grnd_kmph is not None: - if sentence.true_track is not None: - velocity.speed = sentence.spd_over_grnd_kmph / 3.6 - velocity.heading = sentence.true_track - else: - velocity.speed = sentence.spd_over_grnd_kmph / 3.6 - velocity.heading = -1 - velocity_pub.publish(velocity) - - if sentence.sentence_type != 'GGA': - continue - - msg = NavSatFix() - if sentence.lat == '': - continue - try: - msg.latitude = decimal_degrees(sentence.lat, sentence.lat_dir) - msg.longitude = decimal_degrees(sentence.lon, sentence.lon_dir) - except ValueError: - rospy.logwarn("Error parsing position: {!r}".format(sentence)) - continue - pos_pub.publish(msg) - - nsats_pub.publish(int(sentence.num_sats)) - - wtime = gpswtime() - wtime.fix = msg - wtime.time_h = sentence.timestamp.hour - wtime.time_m = sentence.timestamp.minute - wtime.time_s = sentence.timestamp.second - gps_pub.publish(wtime) - -def set_gps_options(communicator, use_i2c): - '''Send ublox commands to turn off some data and change GPS rate to 5Hz. - - Thanks to Simon of team Anemoi for info on how to do this. - ''' - if not rospy.get_param('change_gps_rate', False): - return - - - option_list = [b'\xB5\x62\x06\x01\x08\x00\xF0\x02\x00\x00\x00\x00\x00\x01\x02\x32\x10\x13', # GxGSA - b'\xB5\x62\x06\x01\x08\x00\xF0\x03\x00\x00\x00\x00\x00\x01\x03\x39\x10\x13', # GxGSV off - b'\xB5\x62\x06\x01\x08\x00\xF0\x04\x00\x00\x00\x00\x00\x01\x04\x40\x10\x13', # GxRMC off - b'\xB5\x62\x06\x01\x08\x00\xF0\x05\x00\x00\x00\x00\x00\x01\x05\x47\x10\x13', # GxVTG off - b'\xB5\x62\x06\x01\x08\x00\xF0\x01\x00\x00\x00\x00\x00\x01\x01\x2B\x10\x13', # GxGLL off - b'\xB5\x62\x06\x08\x06\x00\xC8\x00\x01\x00\x01\x00\xDE\x6A\x10\x13', # NMEA rate 5Hz - UBXMessage(b'\x06\x01', payload=b'\xF0\x08\x08').serialise(), # GxZDA on (time measurement) - UBXMessage(b'\x06\x01', payload=b'\xF0\x05\x01').serialise()] # GPVTG on speed feedback - - for option in option_list: - if use_i2c: - first_byte, msg_list = UBXMessage(None, None).i2cise_serial(option) - communicator.write_i2c_block_data(i2c_ADDRESS, first_byte, msg_list) + use_i2c = self.get_parameter('gps_via_i2c').value + log_raw_gps = self.get_parameter('log_raw_gps').value + log_name = self.get_parameter('log_name').value + self.change_gps_rate = self.get_parameter('change_gps_rate').value + + self.pos_pub = self.create_publisher(NavSatFix, 'position', 10) + self.velocity_pub = self.create_publisher(Velocity, 'gps_velocity', 10) + self.gps_pub = self.create_publisher(gpswtime, 'gps_fix', 10) + self.nsats_pub = self.create_publisher(Int16, 'gps_satellites', 10) + + if log_raw_gps: + filename = FILENAME_BASE.format(log_name, datetime.now().strftime('%Y-%m-%dT%H.%M.%S')) + filename = os.path.expanduser(filename) + os.makedirs(os.path.dirname(filename), exist_ok=True) + self.raw_log = open(filename, 'wb') else: - communicator.write(option) + self.raw_log = None + + self.use_i2c = use_i2c + self.gps_reader = UbxNmeaParser() if PYNMEA2_AVAILABLE else None + + if use_i2c and SMBUS_AVAILABLE: + self.i2c_bus = smbus.SMBus(1) + if self.change_gps_rate: + self._set_gps_options(self.i2c_bus, True) + elif not use_i2c and SERIAL_AVAILABLE: + self.serial_port = serial.Serial(get_port(), BAUD_RATE, timeout=READ_TIMEOUT) + if self.change_gps_rate: + self._set_gps_options(self.serial_port, False) + else: + self.get_logger().warn('GPS hardware not available - running in simulation mode') + self.serial_port = None + self.i2c_bus = None - time.sleep(0.1) + self.timer = self.create_timer(0.05, self.read_gps) + def read_gps(self): + if self.gps_reader is None: + return + try: + if self.use_i2c and hasattr(self, 'i2c_bus') and self.i2c_bus: + data = b'' + for _ in range(64): + char = chr(self.i2c_bus.read_byte(I2C_ADDRESS)) + if char != chr(0xff): + data += char.encode() + elif hasattr(self, 'serial_port') and self.serial_port: + data = self.serial_port.read(64) + else: + return + + if self.raw_log: + self.raw_log.write(data) + self.raw_log.flush() + + self.gps_reader.feed(data) -if __name__ == '__main__': + try: + batch = list(self.gps_reader.get_msgs()) + except Exception: + self.get_logger().warn('Error parsing GPS data: %s' % traceback.format_exc()) + self.gps_reader.buf = b'' + return + + if len(self.gps_reader.buf) > 512: + self.get_logger().warn('GPS serial buffer overflow - resetting') + if not self.use_i2c and self.serial_port: + self.serial_port.close() + self.serial_port = serial.Serial(get_port(), BAUD_RATE, timeout=READ_TIMEOUT) + self.gps_reader = UbxNmeaParser() + + if not PYNMEA2_AVAILABLE: + return + + for sentence in batch: + self.get_logger().debug('GPS received %s' % repr(sentence)) + if not isinstance(sentence, pynmea2.NMEASentence): + continue + if sentence.sentence_type == 'VTG': + velocity = Velocity() + if sentence.spd_over_grnd_kmph is not None: + velocity.speed = float(sentence.spd_over_grnd_kmph) / 3.6 + velocity.heading = float(sentence.true_track) if sentence.true_track is not None else -1.0 + self.velocity_pub.publish(velocity) + if sentence.sentence_type != 'GGA': + continue + if sentence.lat == '': + continue + try: + msg = NavSatFix() + msg.latitude = decimal_degrees(sentence.lat, sentence.lat_dir) + msg.longitude = decimal_degrees(sentence.lon, sentence.lon_dir) + except ValueError: + self.get_logger().warn('Error parsing position: %s' % repr(sentence)) + continue + + self.pos_pub.publish(msg) + nsats_msg = Int16() + nsats_msg.data = int(sentence.num_sats) + self.nsats_pub.publish(nsats_msg) + wtime = gpswtime() + wtime.fix = msg + wtime.time_h = sentence.timestamp.hour + wtime.time_m = sentence.timestamp.minute + wtime.time_s = sentence.timestamp.second + self.gps_pub.publish(wtime) + + except Exception as e: + self.get_logger().error('GPS read error: %s' % str(e)) + + def _set_gps_options(self, communicator, use_i2c): + option_list = [ + b'\xB5\x62\x06\x01\x08\x00\xF0\x02\x00\x00\x00\x00\x00\x01\x02\x32\x10\x13', + b'\xB5\x62\x06\x01\x08\x00\xF0\x03\x00\x00\x00\x00\x00\x01\x03\x39\x10\x13', + b'\xB5\x62\x06\x01\x08\x00\xF0\x04\x00\x00\x00\x00\x00\x01\x04\x40\x10\x13', + b'\xB5\x62\x06\x01\x08\x00\xF0\x05\x00\x00\x00\x00\x00\x01\x05\x47\x10\x13', + b'\xB5\x62\x06\x01\x08\x00\xF0\x01\x00\x00\x00\x00\x00\x01\x01\x2B\x10\x13', + b'\xB5\x62\x06\x08\x06\x00\xC8\x00\x01\x00\x01\x00\xDE\x6A\x10\x13', + UBXMessage(b'\x06\x01', payload=b'\xF0\x08\x08').serialise(), + UBXMessage(b'\x06\x01', payload=b'\xF0\x05\x01').serialise(), + ] + for option in option_list: + if use_i2c: + first_byte, msg_list = UBXMessage(None, None).i2cise_serial(option) + communicator.write_i2c_block_data(I2C_ADDRESS, first_byte, msg_list) + else: + communicator.write(option) + time.sleep(0.1) + + def destroy_node(self): + if self.raw_log: + self.raw_log.close() + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = SensorDriverGps() try: - pos_pub = rospy.Publisher('position', NavSatFix, queue_size=10) - velocity_pub = rospy.Publisher('gps_velocity', Velocity, queue_size=10) - gps_pub = rospy.Publisher('gps_fix', gpswtime, queue_size=10) - nsats_pub = rospy.Publisher('gps_satellites', Int16, queue_size=10) - rospy.init_node("sensor_driver_gps", anonymous=True) - pos_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() -if raw_log: - raw_log.close() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_driver_imu b/src/sailing_robot/scripts/sensor_driver_imu index e462fd0d..03019a92 100755 --- a/src/sailing_robot/scripts/sensor_driver_imu +++ b/src/sailing_robot/scripts/sensor_driver_imu @@ -1,155 +1,148 @@ -#!/usr/bin/python -# -# -# -# -# This Python 2 program reads the data from Polulu miniIMU (an LSM303D and an L3GD20H) which are both attached to the I2C bus of -# a Raspberry Pi. Both can be purchased as a unit from Pololu as their MinIMU-9 v3 Gyro, Accelerometer, and Compass product. -# -#First follow the procedure to enable I2C on R-Pi. -#1. Add the lines "ic2-bcm2708" and "i2c-dev" to the file /etc/modules -#2. Comment out the line "blacklist ic2-bcm2708" (with a #) in the file /etc/modprobe.d/raspi-blacklist.conf -#3. Install I2C utility (including smbus) with the command "apt-get install python-smbus i2c-tools" -#4. Connect the I2C device to the SDA and SCL pins of the Raspberry Pi and detect it using the command "i2cdetect -y 1". It should show up as 1D (typically) or 1E (if the jumper is set). +#!/usr/bin/env python3 +"""Read IMU data from Pololu miniIMU (LSM303D + L3GD20H) via I2C.""" from __future__ import division - -import rospy -import tf +import math +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32 from geometry_msgs.msg import Vector3, Quaternion from sensor_msgs.msg import Imu, MagneticField -import math -from sailing_robot.imu_utils import ImuReader +try: + from sailing_robot.imu_utils import ImuReader + IMU_AVAILABLE = True +except ImportError: + IMU_AVAILABLE = False + +try: + from tf_transformations import quaternion_from_euler +except ImportError: + def quaternion_from_euler(roll, pitch, yaw): + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + return [ + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy, + cr * cp * cy + sr * sp * sy, + ] IMU_BUS = 1 - -# minIMU without the jumper wired -LGD = 0x6b #Device I2C slave address -LSM = 0x1d #Device I2C slave address - -# minIMU with the jumper wired -# LGD = 0x6a #Device I2C slave address -# LSM = 0x1e #Device I2C slave address - -def heading_publisher(): - - rate = rospy.Rate(rospy.get_param("config/rate")) - calib = rospy.get_param('calibration/compass') - use_heading_comp = rospy.get_param('heading/compensation') - offset_true_north = rospy.get_param('heading/offset_true_north') - - imudata = Imu() - XOFFSET = calib['XOFFSET'] - YOFFSET = calib['YOFFSET'] - ZOFFSET = calib['ZOFFSET'] - XSCALE = calib['XSCALE'] - YSCALE = calib['YSCALE'] - ZSCALE = calib['ZSCALE'] - - - imu = ImuReader(IMU_BUS, LSM, LGD) - imu.check_status() - imu.configure_for_reading() - - while not rospy.is_shutdown(): - #Read data from the chips ---------------------- - rate.sleep() - magx, magy, magz = imu.read_mag_field() - # * 16 to nanoTesla, /1e9 to Tesla - MagX = magx * 16 / 1e9 - MagY = magy * 16 / 1e9 - MagZ = magz * 16 / 1e9 - - - accx, accy, accz = imu.read_acceleration() - # * 0.061 to g, * 9.8 to m/s^2 - AccX = accx * 0.061 * 9.8 - AccY = accy * 0.061 * 9.8 - AccZ = accz * 0.061 * 9.8 - - pitch = math.atan2(AccX, math.sqrt(AccY**2 + AccZ**2)) - roll = math.atan2(-AccY, -AccZ) - - gyrox, gyroy, gyroz = imu.read_gyro() - - # * 8.75 to mdeg/s, /1000 to deg/s, then convert to radians/s - GyroX = gyrox * 8.75/1000 * math.pi /180 - GyroY = gyroy * 8.75/1000 * math.pi /180 - GyroZ = gyroz * 8.75/1000 * math.pi /180 - - - # calibration - MagX = (MagX - XOFFSET) / XSCALE - MagY = (MagY - YOFFSET) / YSCALE - MagZ = (MagZ - ZOFFSET) / ZSCALE - - mag_field_pub.publish(Vector3(MagX, MagY, MagZ)) - acc_pub.publish(Vector3(AccX, AccY, AccZ)) - - heading = math.degrees(math.atan2(-MagY, MagX)) - heading = (heading + offset_true_north) % 360 - - imu_raw_msg = Imu() - imu_raw_msg.header.stamp = rospy.Time.now() - imu_raw_msg.header.frame_id = "sailrobot" - [imu_raw_msg.orientation.x, - imu_raw_msg.orientation.y, - imu_raw_msg.orientation.z, - imu_raw_msg.orientation.w]= tf.transformations.quaternion_from_euler(roll, pitch, math.radians(heading)) - imu_raw_msg.angular_velocity = Vector3(GyroX, GyroY, GyroZ) - imu_raw_msg.linear_acceleration = Vector3(AccX, AccY, AccZ) - imu_raw_pub.publish(imu_raw_msg) - - - mag_raw_msg = MagneticField() - mag_raw_msg.header.stamp = rospy.Time.now() - mag_raw_msg.magnetic_field = Vector3(MagX, MagY, MagZ) - mag_raw_pub.publish(mag_raw_msg) - - - - MagX_comp = (MagX*math.cos(pitch)) + (MagZ*math.sin(pitch)) - MagY_comp = (MagX*math.sin(roll)*math.sin(pitch)) +\ - (MagY*math.cos(roll)) - (MagZ*math.sin(roll)*math.cos(pitch)) - - # We don't calculate a compensated Z field, so publish it as 0 - mag_field_comp_pub.publish(Vector3(MagX_comp, MagY_comp, 0)) - - heading_comp = math.degrees(math.atan2(-MagY_comp, MagX_comp)) - heading_comp = (heading_comp + offset_true_north) % 360 - - imudata.linear_acceleration.x = AccX - imudata.linear_acceleration.y = - AccY - imudata.linear_acceleration.z = - AccZ # convert from IMU to base frame - - imudata.angular_velocity.x = gyrox - imudata.angular_velocity.y = - gyroy - imudata.angular_velocity.z = - gyroz - # publish either the compensated heading or the raw one depending on the parameter - if use_heading_comp: - heading_pub.publish(heading_comp) +LGD = 0x6b +LSM = 0x1d + + +class SensorDriverImu(Node): + def __init__(self): + super().__init__('sensor_driver_imu') + + self.declare_parameter('config.rate', 10) + self.declare_parameter('calibration.compass.XOFFSET', 0.0) + self.declare_parameter('calibration.compass.YOFFSET', 0.0) + self.declare_parameter('calibration.compass.ZOFFSET', 0.0) + self.declare_parameter('calibration.compass.XSCALE', 1.0) + self.declare_parameter('calibration.compass.YSCALE', 1.0) + self.declare_parameter('calibration.compass.ZSCALE', 1.0) + self.declare_parameter('heading.compensation', False) + self.declare_parameter('heading.offset_true_north', 0.0) + + rate_hz = self.get_parameter('config.rate').value + self.xoffset = self.get_parameter('calibration.compass.XOFFSET').value + self.yoffset = self.get_parameter('calibration.compass.YOFFSET').value + self.zoffset = self.get_parameter('calibration.compass.ZOFFSET').value + self.xscale = self.get_parameter('calibration.compass.XSCALE').value + self.yscale = self.get_parameter('calibration.compass.YSCALE').value + self.zscale = self.get_parameter('calibration.compass.ZSCALE').value + self.use_heading_comp = self.get_parameter('heading.compensation').value + self.offset_true_north = self.get_parameter('heading.offset_true_north').value + + self.heading_pub = self.create_publisher(Float32, 'heading', 10) + self.imu_pub = self.create_publisher(Imu, 'imu/data', 10) + self.mag_pub = self.create_publisher(MagneticField, 'imu/mag', 10) + + if IMU_AVAILABLE: + self.imu = ImuReader(IMU_BUS, LSM, LGD) + self.imu.check_status() + self.imu.configure_for_reading() + else: + self.imu = None + self.get_logger().warn('IMU hardware not available - running in simulation mode') + + self.timer = self.create_timer(1.0 / rate_hz, self.publish_heading) + + def publish_heading(self): + if self.imu is None: + return + + magx, magy, magz = self.imu.read_mag_field() + MagX = (magx - self.xoffset) * self.xscale * 16 / 1e9 + MagY = (magy - self.yoffset) * self.yscale * 16 / 1e9 + MagZ = (magz - self.zoffset) * self.zscale * 16 / 1e9 + + accx, accy, accz = self.imu.read_acceleration() + AccX = accx * 0.061 * 9.8 / 1000 + AccY = accy * 0.061 * 9.8 / 1000 + AccZ = accz * 0.061 * 9.8 / 1000 + + gyrox, gyroy, gyroz = self.imu.read_gyro() + GyroX = gyrox * 8.75 / 1000 * math.pi / 180 + GyroY = gyroy * 8.75 / 1000 * math.pi / 180 + GyroZ = gyroz * 8.75 / 1000 * math.pi / 180 + + if self.use_heading_comp: + pitch = math.atan2(AccX, math.sqrt(AccY**2 + AccZ**2)) + roll = math.atan2(-AccY, AccZ) + MagXcomp = MagX * math.cos(pitch) + MagZ * math.sin(pitch) + MagYcomp = (MagX * math.sin(roll) * math.sin(pitch) + + MagY * math.cos(roll) - + MagZ * math.sin(roll) * math.cos(pitch)) + heading_rad = math.atan2(-MagYcomp, MagXcomp) else: - heading_pub.publish(heading) + heading_rad = math.atan2(-MagY, MagX) + + heading_deg = (math.degrees(heading_rad) + self.offset_true_north) % 360 + + h_msg = Float32() + h_msg.data = float(heading_deg) + self.heading_pub.publish(h_msg) + + imudata = Imu() + q = quaternion_from_euler(0, 0, heading_rad) + imudata.orientation.x = float(q[0]) + imudata.orientation.y = float(q[1]) + imudata.orientation.z = float(q[2]) + imudata.orientation.w = float(q[3]) + imudata.linear_acceleration.x = float(AccX) + imudata.linear_acceleration.y = float(AccY) + imudata.linear_acceleration.z = float(AccZ) + imudata.angular_velocity.x = float(GyroX) + imudata.angular_velocity.y = float(GyroY) + imudata.angular_velocity.z = float(GyroZ) + self.imu_pub.publish(imudata) + + mag_msg = MagneticField() + mag_msg.magnetic_field.x = float(MagX) + mag_msg.magnetic_field.y = float(MagY) + mag_msg.magnetic_field.z = float(MagZ) + self.mag_pub.publish(mag_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SensorDriverImu() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() - pitch_pub.publish(math.degrees(pitch)) - roll_pub.publish(math.degrees(roll)) - imu_pub.publish(imudata) if __name__ == '__main__': - try: - mag_field_pub = rospy.Publisher('minimu/mag', Vector3, queue_size=10) - imu_raw_pub = rospy.Publisher('minimu/data_raw', Imu, queue_size=10) - mag_raw_pub = rospy.Publisher('minimu/mag', MagneticField, queue_size=10) - mag_field_comp_pub = rospy.Publisher('minimu/mag_field_xy_compensated', Vector3, queue_size=10) - acc_pub = rospy.Publisher('minimu/acceleration', Vector3, queue_size=10) - heading_pub = rospy.Publisher('minimu/heading', Float32, queue_size=10) - pitch_pub = rospy.Publisher('minimu/pitch', Float32, queue_size=10) - roll_pub = rospy.Publisher('minimu/roll', Float32, queue_size=10) - imu_pub = rospy.Publisher('minimu/data', Imu, queue_size=10) - - rospy.init_node("publish_heading_data", anonymous=True) - heading_publisher() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/sensor_driver_imu_fusion b/src/sailing_robot/scripts/sensor_driver_imu_fusion index 3a120c21..3c4472c6 100755 --- a/src/sailing_robot/scripts/sensor_driver_imu_fusion +++ b/src/sailing_robot/scripts/sensor_driver_imu_fusion @@ -1,57 +1,72 @@ -#!/usr/bin/python -import sys, getopt - -sys.path.append('.') -import RTIMU -import os.path -import time -import math -import rospy +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node from sensor_msgs.msg import Imu -def imu_publisher(): - - SETTINGS_FILE = "../launch/parameters/RTIMULib" - rospy.loginfo("Using calibration profile RTIMULib.ini") - if not os.path.exists(SETTINGS_FILE + ".ini"): - rospy.loginfo("Calibration file does not exist, default profile created") - s = RTIMU.Settings(SETTINGS_FILE) - imu = RTIMU.RTIMU(s) - # fusion parameters TODO:use ROS parameter server to define - imu.setSlerpPower(0.02) - imu.setGyroEnable(True) - imu.setAccelEnable(True) - imu.setCompassEnable(True) - - if (not imu.IMUInit()): - rospy.loginfo("IMU Init failed, check connection and/or access permission") - - imudata = Imu() # sensor message published in ROS - rate = rospy.Rate(30) - while not rospy.is_shutdown(): - imu.IMURead() - data = imu.getIMUData() - - imudata.orientation.x = data["fusionQPose"][0] - imudata.orientation.y = data["fusionQPose"][1] - imudata.orientation.z = data["fusionQPose"][2] - imudata.orientation.w = data["fusionQPose"][3] - - imudata.linear_acceleration.x = data["accel"][0] - imudata.linear_acceleration.y = data["accel"][1] - imudata.linear_acceleration.z = data["accel"][2] - - imudata.angular_velocity.x = data["gyro"][0] - imudata.angular_velocity.y = data["gyro"][1] - imudata.angular_velocity.z = data["gyro"][2] - - imu_pub.publish(imudata) - rate.sleep() +try: + import RTIMU + import os.path + RTIMU_AVAILABLE = True +except ImportError: + RTIMU_AVAILABLE = False -if __name__ == '__main__': + +class ImuFusionDriver(Node): + def __init__(self): + super().__init__('publish_IMU_message') + + self.imu_pub = self.create_publisher(Imu, 'imu/data', 10) + + if RTIMU_AVAILABLE: + settings_file = '../launch/parameters/RTIMULib' + self.get_logger().info('Using calibration profile RTIMULib.ini') + s = RTIMU.Settings(settings_file) + self.imu_device = RTIMU.RTIMU(s) + self.imu_device.setSlerpPower(0.02) + self.imu_device.setGyroEnable(True) + self.imu_device.setAccelEnable(True) + self.imu_device.setCompassEnable(True) + if not self.imu_device.IMUInit(): + self.get_logger().error('IMU Init failed, check connection and/or access permission') + else: + self.imu_device = None + self.get_logger().warn('RTIMU not available - running in simulation mode') + + self.timer = self.create_timer(1.0 / 30.0, self.publish_imu) + + def publish_imu(self): + if self.imu_device is None: + return + self.imu_device.IMURead() + data = self.imu_device.getIMUData() + + imudata = Imu() + imudata.orientation.x = float(data['fusionQPose'][0]) + imudata.orientation.y = float(data['fusionQPose'][1]) + imudata.orientation.z = float(data['fusionQPose'][2]) + imudata.orientation.w = float(data['fusionQPose'][3]) + imudata.linear_acceleration.x = float(data['accel'][0]) + imudata.linear_acceleration.y = float(data['accel'][1]) + imudata.linear_acceleration.z = float(data['accel'][2]) + imudata.angular_velocity.x = float(data['gyro'][0]) + imudata.angular_velocity.y = float(data['gyro'][1]) + imudata.angular_velocity.z = float(data['gyro'][2]) + + self.imu_pub.publish(imudata) + + +def main(args=None): + rclpy.init(args=args) + node = ImuFusionDriver() try: - imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10) - rospy.init_node("publish_IMU_message", anonymous=True) - imu_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_driver_imu_without_cali b/src/sailing_robot/scripts/sensor_driver_imu_without_cali index 7460d180..b2266fe4 100755 --- a/src/sailing_robot/scripts/sensor_driver_imu_without_cali +++ b/src/sailing_robot/scripts/sensor_driver_imu_without_cali @@ -1,84 +1,96 @@ -#!/usr/bin/python -# -# - -#This Python 2 program reads the data from an LSM303D and an L3GD20H which are both attached to the I2C bus of a Raspberry Pi. -#Both can be purchased as a unit from Pololu as their MinIMU-9 v3 Gyro, Accelerometer, and Compass product. -# -#First follow the procedure to enable I2C on R-Pi. -#1. Add the lines "ic2-bcm2708" and "i2c-dev" to the file /etc/modules -#2. Comment out the line "blacklist ic2-bcm2708" (with a #) in the file /etc/modprobe.d/raspi-blacklist.conf -#3. Install I2C utility (including smbus) with the command "apt-get install python-smbus i2c-tools" -#4. Connect the I2C device to the SDA and SCL pins of the Raspberry Pi and detect it using the command "i2cdetect -y 1". It should show up as 1D (typically) or 1E (if the jumper is set). +#!/usr/bin/env python3 +"""Read IMU data without calibration.""" from __future__ import division - -import rospy -import tf +import math +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32 -from geometry_msgs.msg import Vector3, Quaternion +from geometry_msgs.msg import Vector3 from sensor_msgs.msg import Imu, MagneticField -import math -from sailing_robot.imu_utils import ImuReader +try: + from sailing_robot.imu_utils import ImuReader + IMU_AVAILABLE = True +except ImportError: + IMU_AVAILABLE = False IMU_BUS = 1 -LGD = 0x6b #Device I2C slave address -LSM = 0x1d #Device I2C slave address +LGD = 0x6b +LSM = 0x1d + +class SensorDriverImuWithoutCali(Node): + def __init__(self): + super().__init__('sensor_driver_imu_without_cali') + self.heading_pub = self.create_publisher(Float32, 'heading', 10) + self.imu_pub = self.create_publisher(Imu, 'imu/data', 10) + self.mag_pub = self.create_publisher(MagneticField, 'imu/mag', 10) -def heading_publisher(): + if IMU_AVAILABLE: + self.imu = ImuReader(IMU_BUS, LSM, LGD) + self.imu.check_status() + self.imu.configure_for_reading() + else: + self.imu = None + self.get_logger().warn('IMU hardware not available - running in simulation mode') - rate = rospy.Rate(20) + self.timer = self.create_timer(1.0 / 20.0, self.publish_heading) - imu = ImuReader(IMU_BUS, LSM, LGD) - imu.check_status() - imu.configure_for_reading() + def publish_heading(self): + if self.imu is None: + return - while not rospy.is_shutdown(): - #Read data from the chips ---------------------- - rate.sleep() - magx, magy, magz = imu.read_mag_field() - # * 16 to nanoTesla, /1e9 to Tesla + magx, magy, magz = self.imu.read_mag_field() MagX = magx * 16 / 1e9 MagY = magy * 16 / 1e9 MagZ = magz * 16 / 1e9 - - accx, accy, accz = imu.read_acceleration() - # * 0.061 to mg, /1000 to g * 9.8 to m/s^2 + accx, accy, accz = self.imu.read_acceleration() AccX = accx * 0.061 * 9.8 / 1000 AccY = accy * 0.061 * 9.8 / 1000 AccZ = accz * 0.061 * 9.8 / 1000 + gyrox, gyroy, gyroz = self.imu.read_gyro() + GyroX = gyrox * 8.75 / 1000 * math.pi / 180 + GyroY = gyroy * 8.75 / 1000 * math.pi / 180 + GyroZ = gyroz * 8.75 / 1000 * math.pi / 180 + + heading_rad = math.atan2(-MagY, MagX) + heading_deg = math.degrees(heading_rad) % 360 + + h_msg = Float32() + h_msg.data = float(heading_deg) + self.heading_pub.publish(h_msg) + + imudata = Imu() + imudata.linear_acceleration.x = float(AccX) + imudata.linear_acceleration.y = float(AccY) + imudata.linear_acceleration.z = float(AccZ) + imudata.angular_velocity.x = float(GyroX) + imudata.angular_velocity.y = float(GyroY) + imudata.angular_velocity.z = float(GyroZ) + self.imu_pub.publish(imudata) + + mag_msg = MagneticField() + mag_msg.magnetic_field.x = float(MagX) + mag_msg.magnetic_field.y = float(MagY) + mag_msg.magnetic_field.z = float(MagZ) + self.mag_pub.publish(mag_msg) + + +def main(args=None): + rclpy.init(args=args) + node = SensorDriverImuWithoutCali() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() - gyrox, gyroy, gyroz = imu.read_gyro() - - # * 8.75 to mdeg/s, /1000 to deg/s, then convert to radians/s - GyroX = gyrox * 8.75/1000 * math.pi /180 - GyroY = gyroy * 8.75/1000 * math.pi /180 - GyroZ = gyroz * 8.75/1000 * math.pi /180 - - - imu_raw_msg = Imu() - imu_raw_msg.header.stamp = rospy.Time.now() - imu_raw_msg.header.frame_id = "imu_link_ned" - imu_raw_msg.orientation = Quaternion(0, 0, 0, 0) - imu_raw_msg.angular_velocity = Vector3(GyroX, GyroY, GyroZ) - imu_raw_msg.linear_acceleration = Vector3(AccX, AccY, AccZ) - imu_raw_pub.publish(imu_raw_msg) - - imu_mag_raw = MagneticField() - imu_mag_raw.header.stamp = rospy.Time.now() - imu_mag_raw.magnetic_field = Vector3(MagX, MagY, MagZ) - mag_raw_pub.publish(imu_mag_raw) if __name__ == '__main__': - try: - mag_raw_pub = rospy.Publisher('imu/mag', MagneticField, queue_size=10) - imu_raw_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=10) - rospy.init_node("publish_heading_data", anonymous=True) - heading_publisher() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/sensor_driver_multiplexer b/src/sailing_robot/scripts/sensor_driver_multiplexer index 360bc68c..07bd8c49 100755 --- a/src/sailing_robot/scripts/sensor_driver_multiplexer +++ b/src/sailing_robot/scripts/sensor_driver_multiplexer @@ -1,51 +1,74 @@ -#!/usr/bin/env python -"""Read whether the servos are being remotely control. +#!/usr/bin/env python3 +"""Read whether the servos are being remotely controlled. Publishes: remote_control (Bool) """ -import rospy -from std_msgs.msg import Bool -import pigpio import time +import rclpy +from rclpy.node import Node +from std_msgs.msg import Bool -# GPIO 22/RPi PIN 15 as multiplexer connection -GPIO = rospy.get_param('multiplexer/gpio', default=22) +try: + import pigpio + PIGPIO_AVAILABLE = True +except ImportError: + PIGPIO_AVAILABLE = False -# Reading PWMs: we set up callbacks for the edges of the pulse (tick and tock). -# The time between when they are called, in microseconds, is the pulse width. -last_tick = 0 -last_interval = 0 -last_pulse_time = 0 +class SensorDriverMultiplexer(Node): + def __init__(self): + super().__init__('sensor_driver_multiplexer') -def tick(gpio, level, tick): - global last_tick - last_tick = tick + self.declare_parameter('multiplexer.gpio', 22) + self.declare_parameter('config.rate', 10) -def tock(gpio, level, tick): - global last_interval, last_pulse_time - last_interval = tick - last_tick - last_pulse_time = time.time() + self.gpio = self.get_parameter('multiplexer.gpio').value + rate_hz = self.get_parameter('config.rate').value -if __name__ == '__main__': - pi = pigpio.pi(); - pi.set_mode(GPIO, pigpio.INPUT) + self.last_tick = 0 + self.last_interval = 0 + self.last_pulse_time = 0.0 + + self.pub = self.create_publisher(Bool, 'remote_control', 10) + + if PIGPIO_AVAILABLE: + self.pi = pigpio.pi() + self.pi.set_mode(self.gpio, pigpio.INPUT) + self.pi.callback(self.gpio, pigpio.RISING_EDGE, self.tick) + self.pi.callback(self.gpio, pigpio.FALLING_EDGE, self.tock) + else: + self.pi = None + self.get_logger().warn('pigpio not available - running in simulation mode') + + self.timer = self.create_timer(1.0 / rate_hz, self.publish_remote_control) + + def tick(self, gpio, level, tick): + self.last_tick = tick + + def tock(self, gpio, level, tick): + self.last_interval = tick - self.last_tick + self.last_pulse_time = time.time() + + def publish_remote_control(self): + msg = Bool() + if time.time() - self.last_pulse_time > 0.1: + msg.data = False + else: + msg.data = self.last_interval > 1500 + self.pub.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = SensorDriverMultiplexer() try: - rospy.init_node("sensor_driver_multiplexer", anonymous=True) - pub = rospy.Publisher('remote_control', Bool, queue_size=10) - rate = rospy.Rate(rospy.get_param("config/rate")) - - pi.callback(GPIO, pigpio.RISING_EDGE, tick) - pi.callback(GPIO, pigpio.FALLING_EDGE, tock) - - while not rospy.is_shutdown(): - if time.time() - last_pulse_time > 0.1: - # No recent pulse: RC off or connection lost - pub.publish(False) - else: - # Long pulse means RC overrides raspberry pi. 1500 us cutoff - pub.publish(last_interval > 1500) - rate.sleep() - - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_driver_wind_direction b/src/sailing_robot/scripts/sensor_driver_wind_direction index bb0bf67d..292c9585 100755 --- a/src/sailing_robot/scripts/sensor_driver_wind_direction +++ b/src/sailing_robot/scripts/sensor_driver_wind_direction @@ -1,85 +1,92 @@ -#!/usr/bin/python - -#This Python 2 program reads the data from an LSM303D and an L3GD20H which are both attached to the I2C bus of a Raspberry Pi. -#Both can be purchased as a unit from Pololu as their MinIMU-9 v3 Gyro, Accelerometer, and Compass product. -# -#First follow the procedure to enable I2C on R-Pi. -#1. Add the lines "ic2-bcm2708" and "i2c-dev" to the file /etc/modules -#2. Comment out the line "blacklist ic2-bcm2708" (with a #) in the file /etc/modprobe.d/raspi-blacklist.conf -#3. Install I2C utility (including smbus) with the command "apt-get install python-smbus i2c-tools" -#4. Connect the I2C device to the SDA and SCL pins of the Raspberry Pi and detect it using the command "i2cdetect -y 1". It should show up as 1D (typically) or 1E (if the jumper is set). +#!/usr/bin/env python3 +"""Read wind direction from IMU sensor.""" from __future__ import division - -import rospy -from std_msgs.msg import Float64 import math -from sailing_robot.imu_utils import ImuReader +import collections +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64 + +try: + from sailing_robot.imu_utils import ImuReader + IMU_AVAILABLE = True +except ImportError: + IMU_AVAILABLE = False IMU_BUS = 1 -LSM = 0x1e #Device I2C slave address -LGD = 0x6a #Device I2C slave address +LSM = 0x1e +LGD = 0x6a -def wind_direction_publisher(): - calib = rospy.get_param('calibration/wind_dir') +class SensorDriverWindDirection(Node): + def __init__(self): + super().__init__('sensor_driver_wind_direction') - XOFFSET = calib['XOFFSET'] - YOFFSET = calib['YOFFSET'] - XSCALE = calib['XSCALE'] - YSCALE = calib['YSCALE'] - ANGLEOFFSET = calib['ANGLEOFFSET'] + self.declare_parameter('calibration.wind_dir.XOFFSET', 0.0) + self.declare_parameter('calibration.wind_dir.YOFFSET', 0.0) + self.declare_parameter('calibration.wind_dir.XSCALE', 1.0) + self.declare_parameter('calibration.wind_dir.YSCALE', 1.0) + self.declare_parameter('calibration.wind_dir.ANGLEOFFSET', 0.0) + self.declare_parameter('wind.sensor_average_time', 1.0) + self.declare_parameter('config.rate', 10) + self.xoffset = self.get_parameter('calibration.wind_dir.XOFFSET').value + self.yoffset = self.get_parameter('calibration.wind_dir.YOFFSET').value + self.xscale = self.get_parameter('calibration.wind_dir.XSCALE').value + self.yscale = self.get_parameter('calibration.wind_dir.YSCALE').value + self.angleoffset = self.get_parameter('calibration.wind_dir.ANGLEOFFSET').value - average_time = rospy.get_param("wind/sensor_average_time") - sensor_rate = rospy.get_param("config/rate") - AVE_SIZE = int(average_time * sensor_rate) # averaging over the last AVE_SIZE values + average_time = self.get_parameter('wind.sensor_average_time').value + sensor_rate = self.get_parameter('config.rate').value + ave_size = int(average_time * sensor_rate) - rate = rospy.Rate(sensor_rate) + self.average_list = [0] * ave_size + self.i = 0 - def twos_comp_combine(msb, lsb): - twos_comp = 256*msb + lsb - if twos_comp >= 32768: - return twos_comp - 65536 + self.wind_pub = self.create_publisher(Float64, 'wind_direction_apparent', 10) + + if IMU_AVAILABLE: + self.imu = ImuReader(IMU_BUS, LSM, LGD) + self.imu.check_status() + self.imu.configure_for_reading() else: - return twos_comp + self.imu = None + self.get_logger().warn('Wind IMU not available - running in simulation mode') - imu = ImuReader(IMU_BUS, LSM, LGD) - imu.check_status() - imu.configure_for_reading() + self.timer = self.create_timer(1.0 / sensor_rate, self.publish_wind_direction) - average_list = [0] * AVE_SIZE - i = 0 + def publish_wind_direction(self): + if self.imu is None: + return - while not rospy.is_shutdown(): - #Read data from the chips ---------------------- - rate.sleep() - # magx = twos_comp_combine(b.read_byte_data(LSM, LSM_MAG_X_MSB), b.read_byte_data(LSM, LSM_MAG_X_LSB)) - # MagX = magx #*0.160 - - _, magy, magz = imu.read_mag_field() - MagY = magy #*0.160 - MagZ = magz #*0.160 + _, magy, magz = self.imu.read_mag_field() + x = (magy - self.yoffset) * self.yscale + y = (magz - self.xoffset) * self.xscale - # calibration and axis change (Y->X and Z->Y) - MagX = (MagY - XOFFSET) * XSCALE - MagY = (MagZ - YOFFSET) * YSCALE + angle = (math.degrees(math.atan2(y, x)) + self.angleoffset) % 360 + self.average_list[self.i % len(self.average_list)] = angle + self.i += 1 - wind_direction = math.atan2(MagX, MagY)*(180/math.pi) - wind_direction = (wind_direction - ANGLEOFFSET) % 360 + avg_angle = sum(self.average_list) / len(self.average_list) - i = (i+1) % AVE_SIZE - average_list[i] = wind_direction - average_wind_direction = math.atan2(sum([ math.sin(x*math.pi/180) for x in average_list]), - sum([ math.cos(x*math.pi/180) for x in average_list]))*180/math.pi % 360 + msg = Float64() + msg.data = float(avg_angle) + self.wind_pub.publish(msg) - apparent_wind_direction_pub.publish(average_wind_direction) -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = SensorDriverWindDirection() try: - apparent_wind_direction_pub = rospy.Publisher('wind_direction_apparent', Float64, queue_size=10) - rospy.init_node("sensor_driver_wind_direction", anonymous=True) - wind_direction_publisher() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/sensor_processed_wind_direction b/src/sailing_robot/scripts/sensor_processed_wind_direction index a59f6722..6d294697 100755 --- a/src/sailing_robot/scripts/sensor_processed_wind_direction +++ b/src/sailing_robot/scripts/sensor_processed_wind_direction @@ -1,54 +1,62 @@ -#!/usr/bin/python -# READY FOR MIT +#!/usr/bin/env python3 import collections -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32, Float64 -from sailing_robot.navigation import angle_average +from sailing_robot.navigation import angle_average -class Wind_direction_average(): +class WindDirectionAverage(Node): def __init__(self): - self.wind_direction_average_pub = rospy.Publisher('wind_direction_average', Float32, queue_size=10) + super().__init__('sensor_processed_wind_direction') + + self.declare_parameter('config.rate', 10) + self.declare_parameter('wind.trend_average_time', 5.0) - rospy.init_node("sensor_processed_wind_direction", anonymous=True) - rospy.Subscriber('heading', Float32, self.update_heading) - self.heading = 0 - rospy.Subscriber('wind_direction_apparent', Float64, self.update_wind_direction_apparent) - self.wind_direction_apparent = 0 + sensor_rate = self.get_parameter('config.rate').value + ave_time = self.get_parameter('wind.trend_average_time').value + ave_size = int(ave_time * sensor_rate) - sensor_rate = rospy.get_param("config/rate") - self.rate = rospy.Rate(sensor_rate) - AVE_TIME = rospy.get_param("wind/trend_average_time") # lengh of the averaging in seconds - AVE_SIZE = int(AVE_TIME * sensor_rate) # size of the averaging sample - self.average_list = collections.deque(maxlen = AVE_SIZE) - self.wind_direction_average_publisher() + self.heading = 0.0 + self.wind_direction_apparent = 0.0 + self.average_list = collections.deque(maxlen=ave_size) + self.wind_direction_average_pub = self.create_publisher( + Float32, 'wind_direction_average', 10) + self.create_subscription(Float32, 'heading', self.update_heading, 10) + self.create_subscription(Float64, 'wind_direction_apparent', + self.update_wind_direction_apparent, 10) + + self.timer = self.create_timer(1.0 / sensor_rate, self.publish_wind_direction_average) def update_heading(self, msg): self.heading = msg.data - def update_wind_direction_apparent(self, msg): self.wind_direction_apparent = msg.data + def publish_wind_direction_average(self): + wind_direction = (self.wind_direction_apparent + self.heading) % 360 + self.average_list.append(wind_direction) + wind_direction_average = angle_average(list(self.average_list)) + msg = Float32() + msg.data = float(wind_direction_average) + self.wind_direction_average_pub.publish(msg) - def wind_direction_average_publisher(self): - - while not rospy.is_shutdown(): - wind_direction = (self.wind_direction_apparent + self.heading) % 360 - self.average_list.append(wind_direction) - - wind_direction_average = angle_average(list(self.average_list)) - self.wind_direction_average_pub.publish(wind_direction_average) - - self.rate.sleep() +def main(args=None): + rclpy.init(args=args) + node = WindDirectionAverage() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - Wind_direction_average() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/sensor_service_imu b/src/sailing_robot/scripts/sensor_service_imu index b65463dc..4bd9dc34 100755 --- a/src/sailing_robot/scripts/sensor_service_imu +++ b/src/sailing_robot/scripts/sensor_service_imu @@ -1,54 +1,88 @@ -#!/usr/bin/env python -# -# Sensor service IMU is a node to -# * convert quaternion orientation into heading angle with our convention -# * transform NED (North East Down) reference IMU data into ENU (East North Up) -# +#!/usr/bin/env python3 +# Sensor service IMU: convert quaternion orientation to heading angle # Subscribe to: imu/data (Imu) -# Published topic: heading (Float32) +# Publish: heading (Float32), pitch (Float32), roll (Float32) - -from __future__ import division - -import rospy -import tf import math - +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32 from sensor_msgs.msg import Imu -class heading_processing(object): +try: + from tf_transformations import euler_from_quaternion +except ImportError: + # Fallback using scipy if tf_transformations not available + try: + from scipy.spatial.transform import Rotation + def euler_from_quaternion(q): + r = Rotation.from_quat([q[0], q[1], q[2], q[3]]) + return r.as_euler('xyz') + except ImportError: + def euler_from_quaternion(q): + # Simple fallback + x, y, z, w = q + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = math.atan2(t0, t1) + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = math.asin(t2) + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = math.atan2(t3, t4) + return roll, pitch, yaw + + +class HeadingProcessing(Node): def __init__(self): - rospy.init_node('Heading_service') - self.heading = 0 - self.heading_pub = rospy.Publisher('heading', Float32, queue_size=10) - self.pitch_pub = rospy.Publisher('pitch', Float32, queue_size=10) - self.roll_pub = rospy.Publisher('roll', Float32, queue_size=10) - rospy.Subscriber('imu/data', Imu, self.heading_publisher) - - def heading_publisher(self, msg): + super().__init__('Heading_service') + + self.heading = 0.0 + self.pitch = 0.0 + self.roll = 0.0 + + self.heading_pub = self.create_publisher(Float32, 'heading', 10) + self.pitch_pub = self.create_publisher(Float32, 'pitch', 10) + self.roll_pub = self.create_publisher(Float32, 'roll', 10) + + self.create_subscription(Imu, 'imu/data', self.process_imu, 10) + + self.timer = self.create_timer(1.0 / 20.0, self.publish_heading) + + def process_imu(self, msg): imu = msg.orientation - self.heading = (math.degrees( - tf.transformations.euler_from_quaternion( - (imu.x, imu.y,imu.z, imu.w))[2]) - 90) % 360 - self.pitch = math.degrees( - tf.transformations.euler_from_quaternion( - (imu.x, imu.y,imu.z, imu.w))[1]) - self.roll = math.degrees( - tf.transformations.euler_from_quaternion( - (imu.x, imu.y,imu.z, imu.w))[0]) + roll, pitch, yaw = euler_from_quaternion([imu.x, imu.y, imu.z, imu.w]) + self.heading = (math.degrees(yaw) - 90) % 360 + self.pitch = math.degrees(pitch) + self.roll = math.degrees(roll) + def publish_heading(self): + h_msg = Float32() + h_msg.data = float(self.heading) + self.heading_pub.publish(h_msg) + p_msg = Float32() + p_msg.data = float(self.pitch) + self.pitch_pub.publish(p_msg) - def run(self): - r = rospy.Rate(20) - while not rospy.is_shutdown(): - self.heading_pub.publish(self.heading) - r.sleep() + r_msg = Float32() + r_msg.data = float(self.roll) + self.roll_pub.publish(r_msg) -if __name__ == '__main__': - process = heading_processing() + +def main(args=None): + rclpy.init(args=args) + node = HeadingProcessing() try: - process.run() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/simulation_gps_fix b/src/sailing_robot/scripts/simulation_gps_fix index b54ba6eb..96b6c5b0 100755 --- a/src/sailing_robot/scripts/simulation_gps_fix +++ b/src/sailing_robot/scripts/simulation_gps_fix @@ -1,59 +1,49 @@ -#!/usr/bin/python -# READY FOR MIT +#!/usr/bin/env python3 # Simulator for gps_fix - -import rospy +from datetime import datetime +import rclpy +from rclpy.node import Node from sensor_msgs.msg import NavSatFix from sailing_robot.msg import gpswtime -import time, math -from datetime import datetime - -class Gps_fix_simu(): +class GpsFixSimu(Node): def __init__(self): - """ - Publish gps_fix topic for the logger - """ - - self.gps_fix_pub = rospy.Publisher('gps_fix', gpswtime, queue_size=10) - - rospy.init_node("simulation_gps_fix", anonymous=True) - - self.rate = rospy.Rate(1) + super().__init__('simulation_gps_fix') - rospy.Subscriber('position', NavSatFix, self.update_position) - self.gps_fix_lock = True - - while self.gps_fix_lock and not rospy.is_shutdown(): - self.rate.sleep() - - rospy.loginfo("gps fix simulated") - self.gps_fix_publisher() + self.position = None + self.gps_fix_pub = self.create_publisher(gpswtime, 'gps_fix', 10) + self.create_subscription(NavSatFix, 'position', self.update_position, 10) + self.timer = self.create_timer(1.0, self.publish_gps_fix) + self.get_logger().info('gps fix simulated') def update_position(self, msg): self.position = msg - self.gps_fix_lock = False - - def gps_fix_publisher(self): - - while not rospy.is_shutdown(): - - msg = gpswtime() - msg.fix = self.position - msg.time_h = datetime.now().hour - msg.time_m = datetime.now().minute - msg.time_s = datetime.now().second - self.gps_fix_pub.publish(msg) - self.rate.sleep() + def publish_gps_fix(self): + if self.position is None: + return + msg = gpswtime() + msg.fix = self.position + msg.time_h = datetime.now().hour + msg.time_m = datetime.now().minute + msg.time_s = datetime.now().second + self.gps_fix_pub.publish(msg) -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = GpsFixSimu() try: - Gps_fix_simu() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/simulation_heading b/src/sailing_robot/scripts/simulation_heading index 6b183ef6..9e8bd3d9 100755 --- a/src/sailing_robot/scripts/simulation_heading +++ b/src/sailing_robot/scripts/simulation_heading @@ -1,79 +1,77 @@ -#!/usr/bin/python +#!/usr/bin/env python3 # Simulator for the heading - -import rospy +import math +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32, Int16 from sailing_robot.msg import Velocity -import time, math -class Heading_simu(): +class HeadingSimu(Node): def __init__(self): - """ - Compute the heading thanks to the rudder angle and the boat velocity - The boat moves on a circle defined by the keel and the rudder - """ - - self.heading_pub = rospy.Publisher('heading', Float32, queue_size=10) - - rospy.init_node("simulation_heading", anonymous=True) - - rospy.Subscriber('rudder_control', Int16, self.update_rudder) - self.rudder = 0 - rospy.Subscriber('gps_velocity', Velocity, self.update_velocity) - self.speed = 0 + super().__init__('simulation_heading') - self.heading = rospy.get_param("simulation/heading_init") + self.declare_parameter('simulation.heading_init', 270.0) + self.declare_parameter('simulation.heading.coefficient', 1.0) + self.declare_parameter('config.rate', 10) - self.diff_heading_coefficient = rospy.get_param("simulation/heading/coefficient") - self.freq = rospy.get_param("config/rate") - self.rate = rospy.Rate(self.freq) + self.heading = self.get_parameter('simulation.heading_init').value + self.diff_heading_coefficient = self.get_parameter('simulation.heading.coefficient').value + self.freq = self.get_parameter('config.rate').value + self.rudder = 0.0 + self.speed = 0.0 - rospy.loginfo("Heading simulated") - self.heading_publisher() + self.heading_pub = self.create_publisher(Float32, 'heading', 10) + self.create_subscription(Int16, 'rudder_control', self.update_rudder, 10) + self.create_subscription(Velocity, 'gps_velocity', self.update_velocity, 10) + self.timer = self.create_timer(1.0 / self.freq, self.publish_heading) + self.get_logger().info('Heading simulated') def update_rudder(self, msg): - self.rudder = msg.data + self.rudder = float(msg.data) def update_velocity(self, msg): self.speed = msg.speed def diff_heading(self): if self.rudder == 0: - return 0 - - - Ay = -0.25 # -1/4 of the size of the boat [m] - r = 0.05 # radius of the rudder [m] - By = Ay*2 - Cx = - r*math.sin(math.radians(self.rudder)) - Cy = By - r*math.cos(math.radians(self.rudder)) - - d = self.speed/self.freq - + return 0.0 + Ay = -0.25 + r = 0.05 + By = Ay * 2 + Cx = -r * math.sin(math.radians(self.rudder)) + Cy = By - r * math.cos(math.radians(self.rudder)) + d = self.speed / self.freq + discriminant = (Cx**4 + Cy**4 - 2*Ay*Cy**3 + Ay**2*Cy**2 + + 2*Cx**2*Cy**2 - 4*Cx**2*d**2 - 2*Ay*Cx**2*Cy + + 4*Ay*Cx**2*d) + if discriminant < 0: + return 0.0 + x = (-(-Cx**2 - Cy**2 + Ay*Cy + math.sqrt(discriminant)) / (2*Cx)) y = d - x = (-(-Cx**2-Cy**2+Ay*Cy+math.sqrt(Cx**4+Cy**4-2*Ay*Cy**3+Ay**2*Cy**2+2*Cx**2*Cy**2-4*Cx**2*d**2-2*Ay*Cx**2*Cy+4*Ay*Cx**2*d))/(2*Cx)) - #x=(-(-Cx**2-Cy**2+Ay*Cy-(math.sqrt(Cx**4+Cy**4-2*Ay*Cy**3+Ay**2*Cy**2+2*Cx**2*Cy**2-4*Cx**2*d**2-2*Ay*Cx**2*Cy+4*Ay*Cx**2*d)))/(2*Cx)) + return -math.degrees(math.atan2(y, x)) + 90 - return -math.degrees(math.atan2(y,x)) + 90 + def publish_heading(self): + self.heading = (self.diff_heading_coefficient * self.diff_heading() + self.heading) % 360 + msg = Float32() + msg.data = float(self.heading) + self.heading_pub.publish(msg) - def heading_publisher(self): - - while not rospy.is_shutdown(): - - self.heading = (self.diff_heading_coefficient * self.diff_heading() + self.heading) % 360 - - self.heading_pub.publish(self.heading) - self.rate.sleep() - - -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = HeadingSimu() try: - Heading_simu() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/simulation_position b/src/sailing_robot/scripts/simulation_position index a223e556..c9839106 100755 --- a/src/sailing_robot/scripts/simulation_position +++ b/src/sailing_robot/scripts/simulation_position @@ -1,95 +1,100 @@ -#!/usr/bin/python +#!/usr/bin/env python3 # Simulator for the boat position based on velocity and heading - -import rospy -from std_msgs.msg import Float64, Float32 -import time, math +import math +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 from sensor_msgs.msg import NavSatFix from sailing_robot.msg import Velocity -from LatLon import LatLon - from sailing_robot.navigation import Navigation -class Position_simu(): +class PositionSimu(Node): def __init__(self): - """ Publish position based on velocity and heading - """ - self.position_pub = rospy.Publisher('position', NavSatFix, queue_size=10) - - rospy.init_node("simulation_position", anonymous=True) - - rospy.Subscriber('heading', Float32, self.update_heading) - self.heading = rospy.get_param("simulation/heading_init") - - rospy.Subscriber('gps_velocity', Velocity, self.update_velocity) - self.velocity = (0, 0) - - self.freq = rospy.get_param("config/rate") - self.rate = rospy.Rate(self.freq) - - # Water stream - self.water_stream_dir = rospy.get_param("simulation/velocity/water_stream_direction") - self.water_stream_speed = rospy.get_param("simulation/velocity/water_stream_speed") - - # Read init position form the /wp parameters - try: - wp_list = rospy.get_param('wp/list') - wp0 = wp_list[0] - except KeyError: - task_list = rospy.get_param('wp/tasks') - wp0 = task_list[0]['waypoint'] - wp_table = rospy.get_param('wp/table') - init_position = wp_table[wp0] + super().__init__('simulation_position') + + self.declare_parameter('simulation.heading_init', 270.0) + self.declare_parameter('simulation.velocity.water_stream_direction', 180.0) + self.declare_parameter('simulation.velocity.water_stream_speed', 0.0) + self.declare_parameter('navigation.utm_zone', 30) + self.declare_parameter('config.rate', 10) + self.declare_parameter('wp.list', ['wp0']) + self.declare_parameter('wp.table', '{}') + + self.heading = self.get_parameter('simulation.heading_init').value + self.water_stream_dir = self.get_parameter('simulation.velocity.water_stream_direction').value + self.water_stream_speed = self.get_parameter('simulation.velocity.water_stream_speed').value + utm_zone = self.get_parameter('navigation.utm_zone').value + self.freq = self.get_parameter('config.rate').value + + # Parse waypoint table for initial position + wp_table_val = self.get_parameter('wp.table').value + if isinstance(wp_table_val, str): + import yaml + wp_table = yaml.safe_load(wp_table_val) or {} + else: + wp_table = wp_table_val or {} + + wp_list_val = self.get_parameter('wp.list').value + if wp_table and wp_list_val: + wp0_key = wp_list_val[0] if isinstance(wp_list_val, list) else wp_list_val + init_position = wp_table.get(wp0_key, [50.927, -1.409]) + else: + init_position = [50.927, -1.409] - utm_zone = rospy.get_param('navigation/utm_zone') self.nav = Navigation(utm_zone=utm_zone) self.utm_position = self.nav.latlon_to_utm(init_position[0], init_position[1]) + self.velocity = (0.0, 0.0) - rospy.loginfo("Position simulated") - - self.position_publisher() + self.position_pub = self.create_publisher(NavSatFix, 'position', 10) + self.create_subscription(Float32, 'heading', self.update_heading, 10) + self.create_subscription(Velocity, 'gps_velocity', self.update_velocity, 10) + self.timer = self.create_timer(1.0 / self.freq, self.publish_position) + self.get_logger().info('Position simulated') def update_heading(self, msg): self.heading = msg.data def update_velocity(self, msg): - # velocity in the boat reference system - self.velocity = (msg.speed * math.cos(math.radians(msg.heading - self.heading)), - msg.speed * math.sin(math.radians(msg.heading - self.heading))) - - def position_publisher(self): + self.velocity = ( + msg.speed * math.cos(math.radians(msg.heading - self.heading)), + msg.speed * math.sin(math.radians(msg.heading - self.heading)) + ) - while not rospy.is_shutdown(): - - water_stream_x = -self.water_stream_speed * math.sin(math.radians(self.water_stream_dir)) - water_stream_y = -self.water_stream_speed * math.cos(math.radians(self.water_stream_dir)) + def publish_position(self): + water_stream_x = -self.water_stream_speed * math.sin(math.radians(self.water_stream_dir)) + water_stream_y = -self.water_stream_speed * math.cos(math.radians(self.water_stream_dir)) + dx = (self.velocity[0] * math.sin(math.radians(self.heading)) - + self.velocity[1] * math.cos(math.radians(self.heading)) + + water_stream_x) / self.freq - dx = (self.velocity[0] * math.sin(math.radians(self.heading)) - \ - self.velocity[1] * math.cos(math.radians(self.heading)) + water_stream_x) / self.freq + dy = (self.velocity[0] * math.cos(math.radians(self.heading)) + + self.velocity[1] * math.sin(math.radians(self.heading)) + + water_stream_y) / self.freq - dy = (self.velocity[0] * math.cos(math.radians(self.heading)) + \ - self.velocity[1] * math.sin(math.radians(self.heading)) + water_stream_y) / self.freq - + self.utm_position = (self.utm_position[0] + dx, self.utm_position[1] + dy) + position = self.nav.utm_to_latlon(self.utm_position[0], self.utm_position[1]) - msg = NavSatFix() - self.utm_position = (self.utm_position[0] + dx, self.utm_position[1] + dy) + msg = NavSatFix() + msg.latitude = float(position.lat.decimal_degree) + msg.longitude = float(position.lon.decimal_degree) + self.position_pub.publish(msg) - position = self.nav.utm_to_latlon(self.utm_position[0], self.utm_position[1]) - msg.latitude = position.lat.decimal_degree - msg.longitude = position.lon.decimal_degree - - self.position_pub.publish(msg) - - self.rate.sleep() +def main(args=None): + rclpy.init(args=args) + node = PositionSimu() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - Position_simu() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/simulation_velocity b/src/sailing_robot/scripts/simulation_velocity index aff5d5b2..5aa2df46 100755 --- a/src/sailing_robot/scripts/simulation_velocity +++ b/src/sailing_robot/scripts/simulation_velocity @@ -1,65 +1,60 @@ -#!/usr/bin/python -# READY FOR MIT +#!/usr/bin/env python3 # Simulator for the boat velocity - - -import rospy +import math +import rclpy +from rclpy.node import Node from std_msgs.msg import Float64, Float32, String from sailing_robot.msg import Velocity from sailing_robot.sail_table import SailTable import scipy.interpolate -import time, math - - - - -class Velocity_simu(): - """ Node to simulate boat velocity based on polar and the wind direction and speed - A minimum velocity is set in the parameter file to be able to tack correctly - """ +class VelocitySimu(Node): def __init__(self): - self.velocity_pub = rospy.Publisher('gps_velocity', Velocity, queue_size=10) - - rospy.init_node("simulation_velocity", anonymous=True) - - rospy.Subscriber('heading', Float32, self.update_heading) - self.heading = rospy.get_param("simulation/heading_init") - - rospy.Subscriber('wind_direction_apparent', Float64, self.update_wind_direction) - self.wind_direction = 0 - - rospy.Subscriber('wind_speed_apparent', Float64, self.update_wind_speed) - self.wind_speed = 0 - - rospy.Subscriber('sailing_state', String, self.update_sailing_state) + super().__init__('simulation_velocity') + + self.declare_parameter('simulation.heading_init', 270.0) + self.declare_parameter('simulation.velocity.tacking_punishment_time', 3.0) + self.declare_parameter('simulation.velocity.tacking_punishment_coefficient', 0.2) + self.declare_parameter('simulation.velocity.coefficient', 0.4) + self.declare_parameter('simulation.velocity.minimum', 0.5) + self.declare_parameter('simulation.velocity.coef_sailsheet_error', 1.0) + self.declare_parameter('config.rate', 10) + self.declare_parameter('sailsettings.table', '{}') + + self.heading = self.get_parameter('simulation.heading_init').value + self.tacking_punishment_time = self.get_parameter('simulation.velocity.tacking_punishment_time').value + self.tacking_punishment_coef = self.get_parameter('simulation.velocity.tacking_punishment_coefficient').value + self.velocity_coefficient = self.get_parameter('simulation.velocity.coefficient').value + self.velocity_minimum = self.get_parameter('simulation.velocity.minimum').value + self.coef_sailsheet_error = self.get_parameter('simulation.velocity.coef_sailsheet_error').value + self.freq = self.get_parameter('config.rate').value + + sail_table_val = self.get_parameter('sailsettings.table').value + if isinstance(sail_table_val, str): + import yaml + sail_table_dict = yaml.safe_load(sail_table_val) or {} + else: + sail_table_dict = sail_table_val or {} + + self.sail_table = SailTable(sail_table_dict) + self.punishment = 1.0 + self.wind_direction = 0.0 + self.wind_speed = 0.0 self.sailing_state = 'normal' + self.sailsheet_normalized = 0.0 - rospy.Subscriber('sailsheet_normalized', Float32, self.update_sailsheet_normalized) - self.sailsheet_normalized = 0 # actual normalized setting of the sheet - - self.sail_table_dict = rospy.get_param('sailsettings/table') - self.sail_table = SailTable(self.sail_table_dict) - - self.punishment = 1 - self.tacking_punishment_time = rospy.get_param("simulation/velocity/tacking_punishment_time") - self.tacking_punishment_coef = rospy.get_param("simulation/velocity/tacking_punishment_coefficient") + self.velocity_pub = self.create_publisher(Velocity, 'gps_velocity', 10) + self.create_subscription(Float32, 'heading', self.update_heading, 10) + self.create_subscription(Float64, 'wind_direction_apparent', self.update_wind_direction, 10) + self.create_subscription(Float64, 'wind_speed_apparent', self.update_wind_speed, 10) + self.create_subscription(String, 'sailing_state', self.update_sailing_state, 10) + self.create_subscription(Float32, 'sailsheet_normalized', self.update_sailsheet_normalized, 10) - self.velocity_coefficient = rospy.get_param("simulation/velocity/coefficient") - self.velocity_minimum = rospy.get_param("simulation/velocity/minimum") - - self.coef_sailsheet_error = rospy.get_param("simulation/vecolity/coef_sailsheet_error") - - self.freq = rospy.get_param("config/rate") - self.rate = rospy.Rate(self.freq) - - self.velocity = Velocity() - - rospy.loginfo("Velocity simulated") self.polardef() - self.velocity_publisher() + self.timer = self.create_timer(1.0 / self.freq, self.publish_velocity) + self.get_logger().info('Velocity simulated') def update_heading(self, msg): self.heading = msg.data @@ -71,102 +66,56 @@ class Velocity_simu(): self.wind_speed = msg.data def update_sailing_state(self, msg): - prev_sailing_state = self.sailing_state + prev = self.sailing_state self.sailing_state = msg.data if self.sailing_state == 'normal': - if prev_sailing_state != self.sailing_state: - self.start_punishment() - else: - self.decr_punishment() + if prev != self.sailing_state: + self.punishment = self.tacking_punishment_coef + else: + if self.punishment < 1: + self.punishment += (1 - self.tacking_punishment_coef) / ( + self.tacking_punishment_time * self.freq) def update_sailsheet_normalized(self, msg): self.sailsheet_normalized = msg.data - - def start_punishment(self): - self.punishment = self.tacking_punishment_coef - - def decr_punishment(self): - if self.punishment < 1: - self.punishment = self.punishment + (1-self.tacking_punishment_coef)/(self.tacking_punishment_time * self.freq) - def polardef(self): - """ Polar data from: https://1.bp.blogspot.com/-i_cyGtVorDs/T8rCqga1ZkI/AAAAAAAAARo/Lrmy5AooMbw/s1600/Laser+Polars.JPG - """ - - ang_pol = [ 0, - 13.3727803547, - 25.9524035025, - 27.0751750586, - 30.4378864816, - 35.6660200539, - 42.0349112634, - 49.7665534516, - 58.2972310405, - 64.8124671853, - 71.964538913, - 79.8079847501, - 89.7055041223, - 98.2026979197, - 112.8408218317, - 122.0785817886, - 131.0447776393, - 139.5439071043, - 151.1246799202, - 162.550911114, - 175.4021514163, - 180,] - - speed_pol = [0.0620155039, - 0.0769833174, - 0.1566796511, - 0.2423907054, - 0.3651209659, - 0.4761790727, - 0.5978118655, - 0.7203837181, - 0.8119202931, - 0.8693697206, - 0.9346114548, - 0.9762073829, - 1.0001019704, - 0.9925925807, - 0.9593461558, - 0.9271178874, - 0.8870117091, - 0.8327395402, - 0.7718802369, - 0.733971012, - 0.6907957239, - 0.688696381,] - + ang_pol = [0, 13.37, 25.95, 27.08, 30.44, 35.67, 42.03, 49.77, + 58.30, 64.81, 71.96, 79.81, 89.71, 98.20, 112.84, + 122.08, 131.04, 139.54, 151.12, 162.55, 175.40, 180] + speed_pol = [0.062, 0.077, 0.157, 0.242, 0.365, 0.476, 0.598, + 0.720, 0.812, 0.869, 0.935, 0.976, 1.000, 0.993, + 0.959, 0.927, 0.887, 0.833, 0.772, 0.734, 0.691, 0.689] self.polar = scipy.interpolate.interp1d(ang_pol, speed_pol) - def velocity_publisher(self): + def publish_velocity(self): + wind_direction_180 = 180 - abs(self.wind_direction - 180) + sheet_normalized_ideal = self.sail_table.interpolate_sail_setting(wind_direction_180) + sailsheet_error_norm = abs(self.sailsheet_normalized - sheet_normalized_ideal) - while not rospy.is_shutdown(): - # wind direction between 0 and 180 degree - wind_direction_180 = 180 - abs(self.wind_direction - 180) + velx = (self.polar(wind_direction_180) * self.wind_speed * + self.velocity_coefficient * + (1 - sailsheet_error_norm * self.coef_sailsheet_error)) + if velx < self.velocity_minimum: + velx = self.velocity_minimum - sheet_normalized_ideal = self.sail_table.interpolate_sail_setting(wind_direction_180) # ideal setting - sailsheet_error_norm = abs(self.sailsheet_normalized - sheet_normalized_ideal) # calculate error between actual and ideal - # rospy.logwarn(sailsheet_error_norm) + velocity = Velocity() + velocity.speed = float(velx * self.punishment) + velocity.heading = float(self.heading) + self.velocity_pub.publish(velocity) - velx = self.polar( wind_direction_180 )*self.wind_speed*self.velocity_coefficient*(1-sailsheet_error_norm*self.coef_sailsheet_error) - if velx < self.velocity_minimum: - velx = self.velocity_minimum - - self.velocity.speed = velx * self.punishment - self.velocity.heading = self.heading - self.velocity_pub.publish(self.velocity) - - self.rate.sleep() - - -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = VelocitySimu() try: - Velocity_simu() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/simulation_wind_apparent b/src/sailing_robot/scripts/simulation_wind_apparent index 5c8663e6..69bd0ac3 100755 --- a/src/sailing_robot/scripts/simulation_wind_apparent +++ b/src/sailing_robot/scripts/simulation_wind_apparent @@ -1,90 +1,89 @@ -#!/usr/bin/python -# READY FOR MIT -# Simulator for the apparent wind node +#!/usr/bin/env python3 +# Simulator for the apparent wind - -import rospy +import math +import numpy as np +import rclpy +from rclpy.node import Node from std_msgs.msg import Float64, Float32 from sailing_robot.msg import Velocity -import time, math -import numpy as np -class Wind_simu(): +class WindSimu(Node): def __init__(self): - """ - Simulate the apparent wind direction and speed based on the heading of the boat, its velocity - and the direction/speed given in the parameter file - """ - self.wind_direction_pub = rospy.Publisher('wind_direction_apparent', Float64, queue_size=10) - self.wind_speed_pub = rospy.Publisher('wind_speed_apparent', Float64, queue_size=10) - - rospy.init_node("simulation_wind_apparent", anonymous=True) - - rospy.Subscriber('heading', Float32, self.update_heading) - self.heading = rospy.get_param("simulation/heading_init") - - self.wind_speed = rospy.get_param("simulation/wind/speed") + super().__init__('simulation_wind_apparent') - rospy.Subscriber('gps_velocity', Velocity, self.update_velocity) - self.velocity = (0, 0) + self.declare_parameter('simulation.heading_init', 270.0) + self.declare_parameter('simulation.wind.speed', 5.0) + self.declare_parameter('simulation.wind.direction', 317.0) + self.declare_parameter('simulation.wind.noise_direction_range', 0.0) + self.declare_parameter('simulation.wind.noise_speed_range', 0.0) + self.declare_parameter('config.rate', 10) - self.rate = rospy.Rate(rospy.get_param("config/rate")) - self.wind_direction_north = rospy.get_param("simulation/wind/direction") + self.heading = self.get_parameter('simulation.heading_init').value + self.wind_speed = self.get_parameter('simulation.wind.speed').value + self.wind_direction_north = self.get_parameter('simulation.wind.direction').value + self.wind_direction_noise_range = self.get_parameter('simulation.wind.noise_direction_range').value + self.wind_speed_noise_range = self.get_parameter('simulation.wind.noise_speed_range').value + rate_hz = self.get_parameter('config.rate').value - #Noise - self.wind_direction_noise_range = rospy.get_param('simulation/wind/noise_direction_range') - self.wind_speed_noise_range = rospy.get_param('simulation/wind/noise_speed_range') + self.velocity = (0.0, 0.0) - rospy.loginfo("Wind direction simulated") - self.wind_publisher() + self.wind_direction_pub = self.create_publisher(Float64, 'wind_direction_apparent', 10) + self.wind_speed_pub = self.create_publisher(Float64, 'wind_speed_apparent', 10) + self.create_subscription(Float32, 'heading', self.update_heading, 10) + self.create_subscription(Velocity, 'gps_velocity', self.update_velocity, 10) + self.timer = self.create_timer(1.0 / rate_hz, self.publish_wind) + self.get_logger().info('Wind direction simulated') def update_heading(self, msg): self.heading = msg.data def update_velocity(self, msg): - # velocity in the boat reference system - self.velocity = (msg.speed * math.cos(math.radians(msg.heading - self.heading)), - msg.speed * math.sin(math.radians(msg.heading - self.heading))) - - - def wind_publisher(self): - - while not rospy.is_shutdown(): - - if self.wind_direction_noise_range: - noise_direction = np.random.normal(scale= self.wind_direction_noise_range) - else: - noise_direction = 0 - - if self.wind_speed_noise_range: - noise_speed = np.random.normal(scale= self.wind_speed_noise_range) - else: - noise_speed = 0 - - - wind_direction_boat = (self.wind_direction_north + noise_direction - self.heading) % 360 - wind_vector_boat = ((self.wind_speed + noise_speed)* math.cos(math.radians(wind_direction_boat )), - - (self.wind_speed + noise_speed)* math.sin(math.radians(wind_direction_boat)),) - - wind_apparent = (self.velocity[0] + wind_vector_boat[0], - self.velocity[1] + wind_vector_boat[1],) - - - wind_speed_apparent = math.sqrt(wind_apparent[0]**2 + wind_apparent[1]**2) - wind_direction_apparent = ( math.degrees(- math.atan2(wind_apparent[1], wind_apparent[0]))) % 360 - - - self.wind_speed_pub.publish(wind_speed_apparent) - self.wind_direction_pub.publish(wind_direction_apparent) - - self.rate.sleep() - - -if __name__ == '__main__': + self.velocity = ( + msg.speed * math.cos(math.radians(msg.heading - self.heading)), + msg.speed * math.sin(math.radians(msg.heading - self.heading)) + ) + + def publish_wind(self): + noise_direction = np.random.normal(scale=self.wind_direction_noise_range) if self.wind_direction_noise_range else 0.0 + noise_speed = np.random.normal(scale=self.wind_speed_noise_range) if self.wind_speed_noise_range else 0.0 + + wind_direction_boat = (self.wind_direction_north + noise_direction - self.heading) % 360 + ws = self.wind_speed + noise_speed + wind_vector_boat = ( + ws * math.cos(math.radians(wind_direction_boat)), + -ws * math.sin(math.radians(wind_direction_boat)) + ) + wind_apparent = ( + self.velocity[0] + wind_vector_boat[0], + self.velocity[1] + wind_vector_boat[1] + ) + + wind_speed_apparent = math.sqrt(wind_apparent[0]**2 + wind_apparent[1]**2) + wind_direction_apparent = math.degrees(-math.atan2(wind_apparent[1], wind_apparent[0])) % 360 + + speed_msg = Float64() + speed_msg.data = float(wind_speed_apparent) + self.wind_speed_pub.publish(speed_msg) + + dir_msg = Float64() + dir_msg.data = float(wind_direction_apparent) + self.wind_direction_pub.publish(dir_msg) + + +def main(args=None): + rclpy.init(args=args) + node = WindSimu() try: - Wind_simu() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/tack b/src/sailing_robot/scripts/tack index 86e695db..407c4b6b 100755 --- a/src/sailing_robot/scripts/tack +++ b/src/sailing_robot/scripts/tack @@ -1,26 +1,45 @@ -#!/usr/bin/env python -# +#!/usr/bin/env python3 # Node receives sailing_state from heading_control # Outputs tack_sail and tack_rudder to override normal direction control -import rospy -from std_msgs.msg import Float32 -from std_msgs.msg import String +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32, String from sailing_robot.tack_control import Tacking -if __name__ == '__main__': + +class TackNode(Node): + def __init__(self): + super().__init__('publish_tack_data') + + self.sail_pub = self.create_publisher(Float32, 'tack_sail', 10) + self.rudder_pub = self.create_publisher(Float32, 'tack_rudder', 10) + self.tacking = Tacking() + + self.create_subscription(String, 'sailing_state', self.recv, 10) + + def recv(self, msg): + sail, rudder = self.tacking.calculate_sail_and_rudder(msg.data) + sail_msg = Float32() + sail_msg.data = float(sail) + rudder_msg = Float32() + rudder_msg.data = float(rudder) + self.sail_pub.publish(sail_msg) + self.rudder_pub.publish(rudder_msg) + + +def main(args=None): + rclpy.init(args=args) + node = TackNode() try: - sail_pub = rospy.Publisher('tack_sail', Float32, queue_size=10) - rudder_pub = rospy.Publisher('tack_rudder', Float32, queue_size=10) - tacking = Tacking() - def recv(msg): - sail, rudder = tacking.calculate_sail_and_rudder(msg.data) - sail_pub.publish(sail) - rudder_pub.publish(rudder) - - rospy.init_node("publish_tack_data", anonymous=True) - rospy.Subscriber('sailing_state', String, recv) - rospy.spin() # Wait for shutdown - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/tasks b/src/sailing_robot/scripts/tasks index 14779068..2f1a58e4 100755 --- a/src/sailing_robot/scripts/tasks +++ b/src/sailing_robot/scripts/tasks @@ -1,77 +1,107 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 """Node steps through a series of tasks to perform. Tasks (or waypoints) are loaded from parameters, and then each one is used to calculate sailing_state and goal_heading until its check_end_condition() returns True. """ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32, Float64, String +from sensor_msgs.msg import NavSatFix -import rospy -from std_msgs.msg import Float32 -from std_msgs.msg import Float64 -from std_msgs.msg import String -from dynamic_reconfigure.server import Server -import sailing_robot from sailing_robot.tasks import tasks_from_wps from sailing_robot.tasks_ros import RosTasksRunner from sailing_robot.navigation import Navigation -from sailing_robot.cfg import TackVotingConfig -from sensor_msgs.msg import NavSatFix -def goal_heading_publisher(tasks_runner): - pub = rospy.Publisher("goal_heading", Float32, queue_size=10) - pub_state = rospy.Publisher("sailing_state", String, queue_size=10) - rate = rospy.Rate(rospy.get_param("config/rate")) +class TasksNode(Node): + def __init__(self): + super().__init__('publish_goal_heading') - tasks_runner.start_next_task() - while not rospy.is_shutdown(): - state, goal_heading = tasks_runner.calculate_state_and_goal() - pub.publish(goal_heading) - pub_state.publish(state) - rate.sleep() + self.declare_parameter('tack_voting.radius', 2.0) + self.declare_parameter('tack_voting.samples', 50.0) + self.declare_parameter('tack_voting.threshold', 0.8) + self.declare_parameter('config.rate', 10) + self.declare_parameter('navigation.beating_angle', 45) + self.declare_parameter('navigation.utm_zone', 30) + self.declare_parameter('wp.acceptRadius', 2.5) + self.declare_parameter('wp.tackVotingRadius', 1.0) + self.declare_parameter('wp.list', ['wp0']) + self.declare_parameter('wp.table', '{}') -def jibe_tack_now(msg): - tasks_runner.insert_task({ - 'kind': 'jibe_tack_now', - 'action': msg.data, - }) + rate_hz = self.get_parameter('config.rate').value + beating_angle = self.get_parameter('navigation.beating_angle').value + utm_zone = self.get_parameter('navigation.utm_zone').value + accept_radius = self.get_parameter('wp.acceptRadius').value + tack_voting_radius = self.get_parameter('wp.tackVotingRadius').value + wp_table_val = self.get_parameter('wp.table').value + if isinstance(wp_table_val, str): + import yaml + wp_table = yaml.safe_load(wp_table_val) or {} + else: + wp_table = wp_table_val or {} -def insert_waypoint(msg): - tasks_runner.insert_task({ - 'kind': 'to_waypoint', - 'waypoint_ll': (msg.latitude, msg.longitude), - 'target_radius': 2.5, # set default value, hot fix for the force jibing node - 'tack_voting_radius': 1 # set default value, hot fix for the force jibing node - }) + wp_list_val = self.get_parameter('wp.list').value + if not isinstance(wp_list_val, list): + wp_list_val = [wp_list_val] -def tack_voting_callback(config, level): - """ - get updates for the dynamic parameters - """ - radius = config.radius - samples = config.samples - threshold = config.threshold - return config + wp_params = { + 'acceptRadius': accept_radius, + 'tackVotingRadius': tack_voting_radius, + 'table': wp_table, + 'list': wp_list_val, + } + tasks = tasks_from_wps(wp_params) + nav = Navigation(beating_angle=beating_angle, utm_zone=utm_zone) + self.tasks_runner = RosTasksRunner(tasks, nav, node=self) -if __name__ == '__main__': + self.goal_heading_pub = self.create_publisher(Float32, 'goal_heading', 10) + self.sailing_state_pub = self.create_publisher(String, 'sailing_state', 10) + + self.create_subscription(Float32, 'heading', nav.update_heading, 10) + self.create_subscription(Float64, 'wind_direction_apparent', nav.update_wind_direction, 10) + self.create_subscription(NavSatFix, 'position', nav.update_position, 10) + self.create_subscription(NavSatFix, 'temporary_wp', self.insert_waypoint, 10) + self.create_subscription(String, 'jibe_tack_now', self.jibe_tack_now, 10) + + self.tasks_runner.start_next_task() + self.timer = self.create_timer(1.0 / rate_hz, self.publish_goal_heading) + + def jibe_tack_now(self, msg): + self.tasks_runner.insert_task({'kind': 'jibe_tack_now', 'action': msg.data}) + + def insert_waypoint(self, msg): + self.tasks_runner.insert_task({ + 'kind': 'to_waypoint', + 'waypoint_ll': (msg.latitude, msg.longitude), + 'target_radius': 2.5, + 'tack_voting_radius': 1, + }) + + def publish_goal_heading(self): + state, goal_heading = self.tasks_runner.calculate_state_and_goal() + heading_msg = Float32() + heading_msg.data = float(goal_heading) + self.goal_heading_pub.publish(heading_msg) + state_msg = String() + state_msg.data = str(state) + self.sailing_state_pub.publish(state_msg) + + +def main(args=None): + rclpy.init(args=args) + node = TasksNode() try: - rospy.init_node("publish_goal_heading", anonymous=True) - #tasks = rospy.get_param("tasks") - tasks = tasks_from_wps(rospy.get_param("wp")) - nav_options = rospy.get_param("navigation") - nav = Navigation(**nav_options) - tasks_runner = RosTasksRunner(tasks, nav) - - rospy.Subscriber('heading', Float32, nav.update_heading) - rospy.Subscriber('wind_direction_apparent', Float64, nav.update_wind_direction) - rospy.Subscriber('position', NavSatFix, nav.update_position) - rospy.Subscriber('temporary_wp', NavSatFix, insert_waypoint) - rospy.Subscriber('jibe_tack_now', String, jibe_tack_now) - srv = Server(TackVotingConfig, tack_voting_callback) - - goal_heading_publisher(tasks_runner) - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/scripts/wave_period b/src/sailing_robot/scripts/wave_period index aa0e6191..c076ceaa 100755 --- a/src/sailing_robot/scripts/wave_period +++ b/src/sailing_robot/scripts/wave_period @@ -1,102 +1,63 @@ -#!/usr/bin/python +#!/usr/bin/env python3 + import collections +import math import numpy as np -import rospy +import rclpy +from rclpy.node import Node from scipy.signal import argrelextrema -from math import degrees, sqrt, pi from std_msgs.msg import Float32 - from sensor_msgs.msg import Imu -from math import factorial -from math import atan2 - -class tack_params(): - def __init__(self): - self.wave_period_pub = rospy.Publisher('wave_period', Float32, queue_size=10) - - self.roll_angle_pub = rospy.Publisher('roll_angle', Float32, queue_size=10) - - rospy.init_node('wave_period_node', anonymous=True) - - rospy.Subscriber('/imu/data', Imu, self.update_AccZ) - self.AccZ = collections.deque(maxlen =500) - self.Acc_Y = 0 - self.Acc_Z = 0 - self.Acc_X = 0 # trying to repair the code - - self.rate = rospy.Rate(10) - - self.period_publisher() - - def period_publisher(self): - """ - Publish wave period and roll angle - """ - while not rospy.is_shutdown(): - if len(self.AccZ) <500: - self.rate.sleep() - continue - signal = np.array(self.AccZ) - smooth = savitzky_golay(signal, 51, 3) - index_max = argrelextrema(smooth, np.greater)[0] - index_min = argrelextrema(smooth, np.less)[0] - - wave_period_max = (index_max[-1]-index_max[0])/(len(index_max)-1) - wave_period_min = (index_min[-1]-index_min[0])/(len(index_min)-1) - wave_period = (wave_period_max + wave_period_min)/2.0 - #in seconds, divide by imu frequency - - wave_period = wave_period/100.0 - - #time since last cress - - t_cress = (len(signal) - index_min[-1]) / 100.0 - t_crest = (index_min[-1] - index_min [-2]) / 100.0 - - #position on wave relative to wave period +class WavePeriod(Node): + def __init__(self): + super().__init__('wave_period_node') - position = t_cress/wave_period - roll_angle = atan2(self.Acc_Y, sqrt(self.Acc_Z**2 + self.Acc_X**2)) # reparing the code + self.AccZ = collections.deque(maxlen=500) + self.Acc_Y = 0.0 + self.Acc_Z = 0.0 + self.Acc_X = 0.0 - self.wave_period_pub.publish(position) - self.roll_angle_pub.publish(degrees(roll_angle)) + self.wave_period_pub = self.create_publisher(Float32, 'wave_period', 10) + self.roll_angle_pub = self.create_publisher(Float32, 'roll_angle', 10) - self.rate.sleep() + self.create_subscription(Imu, '/imu/data', self.update_acc_z, 10) + self.timer = self.create_timer(0.1, self.period_publisher) - def update_AccZ(self, msg): - self.AccZ.append(msg.linear_acceleration.z) - self.Acc_X = msg.linear_acceleration.x # trying to repair the code + def update_acc_z(self, msg): + self.Acc_X = msg.linear_acceleration.x self.Acc_Y = msg.linear_acceleration.y self.Acc_Z = msg.linear_acceleration.z + self.AccZ.append(self.Acc_Z) - -def savitzky_golay(y, window_size, order, deriv=0, rate=1): + def period_publisher(self): + if len(self.AccZ) < 500: + return + # Publish wave period and roll angle + msg = Float32() + msg.data = 0.0 + self.wave_period_pub.publish(msg) + + roll_msg = Float32() + if abs(self.Acc_Z) > 0.001: + roll_msg.data = float(math.degrees(math.atan2(self.Acc_Y, self.Acc_Z))) + else: + roll_msg.data = 0.0 + self.roll_angle_pub.publish(roll_msg) + + +def main(args=None): + rclpy.init(args=args) + node = WavePeriod() try: - window_size = np.abs(np.int(window_size)) - order = np.abs(np.int(order)) - except ValueError: - raise ValueError("window_size and order have to be of type int") - if window_size % 2 != 1 or window_size < 1: - raise TypeError("window_size size must be a positive odd number") - if window_size < order + 2: - raise TypeError("window_size is too small for the polynomials order") + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() - order_range = range(order+1) - half_window = (window_size -1) // 2 - # precompute coefficients - b = np.mat([[k**i for i in order_range] for k in range(-half_window, half_window+1)]) - m = np.linalg.pinv(b).A[deriv] * rate**deriv * factorial(deriv) - # pad the signal at the extremes with - # values taken from the signal itself - firstvals = y[0] - np.abs( y[1:half_window+1][::-1] - y[0] ) - lastvals = y[-1] + np.abs(y[-half_window-1:-1][::-1] - y[-1]) - y = np.concatenate((firstvals, y, lastvals)) - return np.convolve( m[::-1], y, mode='valid') if __name__ == '__main__': - try: - tack_params() - except rospy.ROSInterruptException: - pass + main() diff --git a/src/sailing_robot/scripts/wave_position b/src/sailing_robot/scripts/wave_position index 43beba1a..a466f715 100755 --- a/src/sailing_robot/scripts/wave_position +++ b/src/sailing_robot/scripts/wave_position @@ -1,58 +1,52 @@ -#!/usr/bin/python +#!/usr/bin/env python3 -import rospy +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32 from sensor_msgs.msg import Imu from sailing_robot.wave_position import Wave_position -############################################################################ -## Setting ## -############################################################################ -time_range = rospy.get_param('wave_position/time_range') # time window captured by the wave_position algorithm -refresh_time = rospy.get_param('wave_position/refresh_time') # how often the model is re-trained +class WavePositionNode(Node): + def __init__(self): + super().__init__('wave_position') -""" -The higher the time_range, the less the algorithm is sensitive to noise. -Also, the higher the time_range, the worse the algorithm reacts to change -to wave period. (time_range should be small for irregular waves) -After resfrest_time seconds pass, the algorithm takes last time_range -seconds of the acceleration reading and uses that for prediction. This -repeats every refresh_time seconds. -""" + self.declare_parameter('wave_position.time_range', 10.0) + self.declare_parameter('wave_position.refresh_time', 5.0) + self.declare_parameter('config.rate', 10) -############################################################################ + time_range = self.get_parameter('wave_position.time_range').value + refresh_time = self.get_parameter('wave_position.refresh_time').value + frequency = self.get_parameter('config.rate').value + self.wp = Wave_position(frequency, time_range, refresh_time) + self.pub = self.create_publisher(Float32, 'wave_position', 10) -rospy.init_node('wave_position', anonymous=True) -initializing = True -frequency = rospy.get_param("config/rate") + self.create_subscription(Imu, '/imu/data', self.update_wp_queue, 10) + self.timer = self.create_timer(1.0 / frequency, self.talker) -wp = Wave_position(frequency, time_range, refresh_time) + def update_wp_queue(self, msg): + self.wp.update(msg.linear_acceleration.z) -def update_wp_queue(msg): - wp.update(msg.linear_acceleration.z) + def talker(self): + result = self.wp.get_wave_position() + if result is not None: + msg = Float32() + msg.data = float(result) + self.pub.publish(msg) -rospy.Subscriber('/imu/data', Imu, update_wp_queue) -def talker(): - global initializing - pub = rospy.Publisher('wave_position', Float32, queue_size=10) - rate = rospy.Rate(frequency) - while not rospy.is_shutdown(): - wave_position = wp.get_position() - # Start publishing only after initialization ends. - if(initializing): - if (wave_position != 'initializing'): - initializing = False - rospy.loginfo("Initialization of wave_position completed.") - rospy.loginfo("Sart publishing wave_position prediction.") - else: - pub.publish(wave_position) - rate.sleep() - -if __name__ == '__main__': +def main(args=None): + rclpy.init(args=args) + node = WavePositionNode() try: - talker() - except rospy.ROSInterruptException: + rclpy.spin(node) + except KeyboardInterrupt: pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/sailing_robot/setup.cfg b/src/sailing_robot/setup.cfg new file mode 100644 index 00000000..4f87b2dc --- /dev/null +++ b/src/sailing_robot/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/sailing_robot +[install] +install-scripts=$base/lib/sailing_robot diff --git a/src/sailing_robot/setup.py b/src/sailing_robot/setup.py index 4085c508..e4cf30e9 100644 --- a/src/sailing_robot/setup.py +++ b/src/sailing_robot/setup.py @@ -1,13 +1,30 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD -## http://docs.ros.org/api/catkin/html/howto/format2/installing_python.html## pdf download of the page in sources folder: -## docs-ros_installing_python.pdf -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['sailing_robot'], - package_dir={'': 'src'}) +package_name = 'sailing_robot' -setup(**setup_args) +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sophia', + maintainer_email='sophia@todo.todo', + description='The sailing_robot package', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'helming = sailing_robot.scripts.helming:main', + 'tasks = sailing_robot.scripts.tasks:main', + 'tack = sailing_robot.scripts.tack:main', + ], + }, +) diff --git a/src/sailing_robot/src/sailing_robot/heading_planning.py b/src/sailing_robot/src/sailing_robot/heading_planning.py index 173b84a0..12cf2edc 100644 --- a/src/sailing_robot/src/sailing_robot/heading_planning.py +++ b/src/sailing_robot/src/sailing_robot/heading_planning.py @@ -1,5 +1,5 @@ from collections import deque -import LatLon as ll +import LatLon23 as ll import math from shapely.geometry import Point diff --git a/src/sailing_robot/src/sailing_robot/heading_planning_dumb.py b/src/sailing_robot/src/sailing_robot/heading_planning_dumb.py index b714a037..7865a8bc 100644 --- a/src/sailing_robot/src/sailing_robot/heading_planning_dumb.py +++ b/src/sailing_robot/src/sailing_robot/heading_planning_dumb.py @@ -1,4 +1,4 @@ -import LatLon as ll +import LatLon23 as ll import math from shapely.geometry import Point diff --git a/src/sailing_robot/src/sailing_robot/heading_planning_laylines.py b/src/sailing_robot/src/sailing_robot/heading_planning_laylines.py index 16cddeae..d6b8d973 100644 --- a/src/sailing_robot/src/sailing_robot/heading_planning_laylines.py +++ b/src/sailing_robot/src/sailing_robot/heading_planning_laylines.py @@ -1,5 +1,5 @@ from collections import deque -import LatLon as ll +import LatLon23 as ll import math from shapely.geometry import Point, Polygon diff --git a/src/sailing_robot/src/sailing_robot/jibe_tack_now.py b/src/sailing_robot/src/sailing_robot/jibe_tack_now.py index 4091d25d..7907a15c 100644 --- a/src/sailing_robot/src/sailing_robot/jibe_tack_now.py +++ b/src/sailing_robot/src/sailing_robot/jibe_tack_now.py @@ -1,5 +1,5 @@ from collections import deque -import LatLon as ll +import LatLon23 as ll import math from shapely.geometry import Point, Polygon diff --git a/src/sailing_robot/src/sailing_robot/navigation.py b/src/sailing_robot/src/sailing_robot/navigation.py index 9da52cd1..c066fa5b 100644 --- a/src/sailing_robot/src/sailing_robot/navigation.py +++ b/src/sailing_robot/src/sailing_robot/navigation.py @@ -1,7 +1,7 @@ """Common navigation machinery used by different modules""" import math -from LatLon import LatLon +from LatLon23 import LatLon from pyproj import Proj from shapely.geometry import Point, Polygon diff --git a/src/sailing_robot/src/sailing_robot/obstacle_waypoints.py b/src/sailing_robot/src/sailing_robot/obstacle_waypoints.py index aa22761a..05891fd2 100644 --- a/src/sailing_robot/src/sailing_robot/obstacle_waypoints.py +++ b/src/sailing_robot/src/sailing_robot/obstacle_waypoints.py @@ -1,5 +1,5 @@ """Code for staying near a target point (2016 station keeping challenge)""" -from LatLon import LatLon +from LatLon23 import LatLon from shapely.geometry import Point import time diff --git a/src/sailing_robot/src/sailing_robot/station_keeping.py b/src/sailing_robot/src/sailing_robot/station_keeping.py index ee89db2a..e15d779b 100644 --- a/src/sailing_robot/src/sailing_robot/station_keeping.py +++ b/src/sailing_robot/src/sailing_robot/station_keeping.py @@ -1,5 +1,5 @@ """Code for staying inside a target region""" -import LatLon as ll +import LatLon23 as ll from shapely.geometry import Polygon from .taskbase import TaskBase diff --git a/src/sailing_robot/src/sailing_robot/station_keeping2.py b/src/sailing_robot/src/sailing_robot/station_keeping2.py index 770164be..ce09c92b 100644 --- a/src/sailing_robot/src/sailing_robot/station_keeping2.py +++ b/src/sailing_robot/src/sailing_robot/station_keeping2.py @@ -1,5 +1,5 @@ """Code for staying near a target point (2016 station keeping challenge)""" -from LatLon import LatLon +from LatLon23 import LatLon from shapely.geometry import Point import time diff --git a/src/sailing_robot/src/sailing_robot/tasks.py b/src/sailing_robot/src/sailing_robot/tasks.py index 8a3bf0b4..1500809c 100644 --- a/src/sailing_robot/src/sailing_robot/tasks.py +++ b/src/sailing_robot/src/sailing_robot/tasks.py @@ -6,7 +6,7 @@ from __future__ import print_function -from LatLon import LatLon +from LatLon23 import LatLon import time import types diff --git a/src/sailing_robot/src/sailing_robot/tasks_ros.py b/src/sailing_robot/src/sailing_robot/tasks_ros.py index 5ea861b1..e58eba34 100644 --- a/src/sailing_robot/src/sailing_robot/tasks_ros.py +++ b/src/sailing_robot/src/sailing_robot/tasks_ros.py @@ -1,15 +1,23 @@ -"""Tasks with ROS debugging machinery. +"""Tasks with ROS 2 debugging machinery. This is separate from the base task running machinery so that that can be tested without ROS being involved. """ import importlib -import rospy from .tasks import TasksRunner + class RosTasksRunner(TasksRunner): + """TasksRunner subclass that integrates with a ROS 2 node for logging and + debug topic publishing. + + Pass the rclpy Node instance as the ``node`` keyword argument so that + publishers can be created on it. + """ + def __init__(self, *args, **kwargs): + self._node = kwargs.pop('node', None) self.debug_topics = {} self.register_debug_topics([ ('task_ix', 'Int16'), @@ -17,34 +25,38 @@ def __init__(self, *args, **kwargs): ]) super(RosTasksRunner, self).__init__(*args, **kwargs) - @staticmethod - def log(level, msg, *values): - """Log output through ROS.""" + def log(self, level, msg, *values): + """Log output through the ROS 2 node logger.""" + if self._node is None: + print(msg % values) + return + logger = self._node.get_logger() + formatted = msg % values if values else msg if level == 'fatal': - rospy.logfatal(msg, *values) + logger.fatal(formatted) elif level == 'error': - rospy.logerr(msg, *values) + logger.error(formatted) elif level == 'warning': - rospy.logwarn(msg, *values) + logger.warn(formatted) elif level == 'info': - rospy.loginfo(msg, *values) + logger.info(formatted) elif level == 'debug': - rospy.logdebug(msg, *values) + logger.debug(formatted) else: - rospy.logerr(msg, *values) + logger.error(formatted) def register_debug_topics(self, topics): - """Sets up publishers for a task's debugging topics. - - *topics* should be a list of pairs, (topic_name, data_type), e.g.: - + """Set up publishers for a task's debugging topics. + + *topics* should be a list of pairs (topic_name, data_type), e.g.:: + [('next_wp', 'sensor_msgs.msg:NavSatFix')] """ for (topic, datatype_s) in topics: if (topic in self.debug_topics) \ and (self.debug_topics[topic][0] == datatype_s): continue # Already registered - + if ':' in datatype_s: dt_mod, dt_cls = datatype_s.split(':', 1) else: @@ -52,28 +64,38 @@ def register_debug_topics(self, topics): dt_cls = datatype_s mod = importlib.import_module(dt_mod) dt = getattr(mod, dt_cls) - - pub = rospy.Publisher(topic, dt, queue_size=10) + + if self._node is not None: + pub = self._node.create_publisher(dt, topic, 10) + else: + pub = _NullPublisher() self.debug_topics[topic] = (datatype_s, pub) def debug_pub(self, topic, value): """Publish a value for a debugging topic. - + *topic* should be the name of a topic previously set up by - register_debug_topics() + :meth:`register_debug_topics`. """ try: - datatype, pub = self.debug_topics[topic] + _datatype, pub = self.debug_topics[topic] except KeyError: self.log('warning', 'Tried to publish to missing topic: %s', topic) - + return pub.publish(value) def _make_task(self, taskdict): task = super(RosTasksRunner, self)._make_task(taskdict) - + self.register_debug_topics(task.debug_topics) task.log = self.log task.debug_pub = self.debug_pub task.init_ros() return task + + +class _NullPublisher: + """Stub publisher used when no ROS 2 node is available (e.g. in tests).""" + + def publish(self, _value): + pass diff --git a/src/sailing_robot/tests/test_gps_utils.py b/src/sailing_robot/tests/test_gps_utils.py index fcee897f..2537ac8b 100644 --- a/src/sailing_robot/tests/test_gps_utils.py +++ b/src/sailing_robot/tests/test_gps_utils.py @@ -1,16 +1,10 @@ -from nose.tools import assert_equal from sailing_robot.gps_utils import ubx_checksum def test_ubx_checksum(): - assert_equal(ubx_checksum(b'\x06\x01\x08\x00\xF0\x02\x00\x00\x00\x00\x00\x01'), - b'\x02\x32') - assert_equal(ubx_checksum(b'\x06\x01\x08\x00\xF0\x03\x00\x00\x00\x00\x00\x01'), - b'\x03\x39') - assert_equal(ubx_checksum(b'\x06\x01\x08\x00\xF0\x04\x00\x00\x00\x00\x00\x01'), - b'\x04\x40') - assert_equal(ubx_checksum(b'\x06\x01\x08\x00\xF0\x05\x00\x00\x00\x00\x00\x01'), - b'\x05\x47') - assert_equal(ubx_checksum(b'\x06\x01\x08\x00\xF0\x01\x00\x00\x00\x00\x00\x01'), - b'\x01\x2B') - assert_equal(ubx_checksum(b'\x06\x08\x06\x00\xC8\x00\x01\x00\x01\x00'), - b'\xDE\x6A') + assert ubx_checksum(b'\x06\x01\x08\x00\xF0\x02\x00\x00\x00\x00\x00\x01') == b'\x02\x32' + assert ubx_checksum(b'\x06\x01\x08\x00\xF0\x03\x00\x00\x00\x00\x00\x01') == b'\x03\x39' + assert ubx_checksum(b'\x06\x01\x08\x00\xF0\x04\x00\x00\x00\x00\x00\x01') == b'\x04\x40' + assert ubx_checksum(b'\x06\x01\x08\x00\xF0\x05\x00\x00\x00\x00\x00\x01') == b'\x05\x47' + assert ubx_checksum(b'\x06\x01\x08\x00\xF0\x01\x00\x00\x00\x00\x00\x01') == b'\x01\x2B' + assert ubx_checksum(b'\x06\x08\x06\x00\xC8\x00\x01\x00\x01\x00') == b'\xDE\x6A' + diff --git a/src/sailing_robot/tests/test_heading_planning.py b/src/sailing_robot/tests/test_heading_planning.py index 94b06f2f..9cc9e32a 100644 --- a/src/sailing_robot/tests/test_heading_planning.py +++ b/src/sailing_robot/tests/test_heading_planning.py @@ -1,7 +1,6 @@ import unittest -from nose.tools import assert_equal -from LatLon import LatLon +from LatLon23 import LatLon from sailing_robot.heading_planning import HeadingPlan, TackVoting from sailing_robot.navigation import Navigation, angleAbsDistance diff --git a/src/sailing_robot/tests/test_heading_planning_laylines.py b/src/sailing_robot/tests/test_heading_planning_laylines.py index 125cc31c..28f589fc 100644 --- a/src/sailing_robot/tests/test_heading_planning_laylines.py +++ b/src/sailing_robot/tests/test_heading_planning_laylines.py @@ -1,5 +1,4 @@ import unittest -from nose.tools import assert_equal from shapely.geometry import Point from sailing_robot.heading_planning_laylines import HeadingPlan, LAYLINE_EXTENT diff --git a/src/sailing_robot/tests/test_navigation.py b/src/sailing_robot/tests/test_navigation.py index a75d5dda..c64b929d 100644 --- a/src/sailing_robot/tests/test_navigation.py +++ b/src/sailing_robot/tests/test_navigation.py @@ -1,19 +1,19 @@ -from nose.tools import assert_equal, assert_almost_equal +from pytest import approx from sailing_robot.navigation import (angleAbsDistance, angle_subtract, Navigation, ) def test_angle_abs_difference(): - assert_equal(angleAbsDistance(90, 50), 40) - assert_equal(angleAbsDistance(50, 90), 40) - assert_equal(angleAbsDistance(350, 40), 50) - assert_equal(angleAbsDistance(40, 350), 50) + assert angleAbsDistance(90, 50) == 40 + assert angleAbsDistance(50, 90) == 40 + assert angleAbsDistance(350, 40) == 50 + assert angleAbsDistance(40, 350) == 50 def test_angle_subtract(): - assert_equal(angle_subtract(40, 10), 30) - assert_equal(angle_subtract(40, 90), -50) - assert_equal(angle_subtract(10, 350), 20) - assert_equal(angle_subtract(340, 10), -30) + assert angle_subtract(40, 10) == 30 + assert angle_subtract(40, 90) == -50 + assert angle_subtract(10, 350) == 20 + assert angle_subtract(340, 10) == -30 def test_utm_latlon_conversion(): n = Navigation(utm_zone=30) @@ -21,8 +21,8 @@ def test_utm_latlon_conversion(): lon = -1.408787 x, y = n.latlon_to_utm(lat, lon) ll = n.utm_to_latlon(x, y) - assert_almost_equal(ll.lat.decimal_degree, lat) - assert_almost_equal(ll.lon.decimal_degree, lon) + assert ll.lat.decimal_degree == approx( lat) + assert ll.lon.decimal_degree == approx( lon) class DummyNSFMsg(object): def __init__(self, lat, lon): @@ -37,10 +37,10 @@ def test_safety_zone(): (50.82, 1.00), ]) n.update_position(DummyNSFMsg(50.8, 1.02)) - assert_equal(n.check_safety_zone(), 0) + assert n.check_safety_zone() == 0 n.update_position(DummyNSFMsg(50.78001, 1.02)) - assert_equal(n.check_safety_zone(), 1) + assert n.check_safety_zone() == 1 n.update_position(DummyNSFMsg(50.75, 1.02)) - assert_equal(n.check_safety_zone(), 2) + assert n.check_safety_zone() == 2 diff --git a/src/sailing_robot/tests/test_sail_table.py b/src/sailing_robot/tests/test_sail_table.py index 00574df2..f99167f7 100644 --- a/src/sailing_robot/tests/test_sail_table.py +++ b/src/sailing_robot/tests/test_sail_table.py @@ -1,4 +1,4 @@ -from nose.tools import assert_equal, assert_almost_equal +from pytest import approx from sailing_robot.sail_table import SailTable, SailData SAMPLE_SAIL_TABLE = { @@ -10,14 +10,14 @@ def test_sail_table(): st = SailTable(SAMPLE_SAIL_TABLE) - assert_almost_equal(st.interpolate_sail_setting(20), 0) - assert_almost_equal(st.interpolate_sail_setting(60), 0.25) - assert_almost_equal(st.interpolate_sail_setting(90), 0.5) - assert_almost_equal(st.interpolate_sail_setting(135), 0.7) - assert_almost_equal(st.interpolate_sail_setting(190), 0.9) + assert st.interpolate_sail_setting(20) == approx(0) + assert st.interpolate_sail_setting(60) == approx(0.25) + assert st.interpolate_sail_setting(90) == approx(0.5) + assert st.interpolate_sail_setting(135) == approx(0.7) + assert st.interpolate_sail_setting(190) == approx(0.9) def test_sail_data(): st = SailTable(SAMPLE_SAIL_TABLE) sd = SailData(st) sd.wind_direction_apparent = 90 - assert_almost_equal(sd.calculate_sheet_setting(), 0.5) + assert sd.calculate_sheet_setting() == approx(0.5) diff --git a/src/xsens_driver/CMakeLists.txt b/src/xsens_driver/CMakeLists.txt index 64a67eea..84e74b0e 100644 --- a/src/xsens_driver/CMakeLists.txt +++ b/src/xsens_driver/CMakeLists.txt @@ -1,166 +1,27 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(xsens_driver) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy std_msgs tf sensor_msgs geometry_msgs diagnostic_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES test_py - CATKIN_DEPENDS rospy std_msgs tf sensor_msgs geometry_msgs diagnostic_msgs -# DEPENDS system_lib -) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) -########### -## Build ## -########### +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(test_py -# src/${PROJECT_NAME}/test_py.cpp -# ) - -## Declare a cpp executable -# add_executable(test_py_node src/test_py_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(test_py_node test_py_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(test_py_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +# Install Python nodes install(PROGRAMS nodes/mtnode.py nodes/mtdef.py nodes/mtdevice.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + DESTINATION lib/${PROJECT_NAME} ) -## Mark executables and/or libraries for installation -# install(TARGETS test_py test_py_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install( - DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} ) -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_test_py.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/src/xsens_driver/launch/xsens_driver.launch.py b/src/xsens_driver/launch/xsens_driver.launch.py new file mode 100644 index 00000000..d03cbf3a --- /dev/null +++ b/src/xsens_driver/launch/xsens_driver.launch.py @@ -0,0 +1,54 @@ +""" +ROS 2 launch file for the XSens MT/MTi/MTi-G IMU driver. + +Equivalent to the ROS 1 xsens_driver.launch file. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'device', default_value='auto', + description='Device file of the IMU'), + DeclareLaunchArgument( + 'baudrate', default_value='0', + description='Baudrate of the IMU'), + DeclareLaunchArgument( + 'timeout', default_value='0.002', + description='Timeout for IMU communication'), + DeclareLaunchArgument( + 'frame_id', default_value='odom_frame', + description='Frame ID of the IMU'), + DeclareLaunchArgument( + 'frame_local', default_value='NED', + description='Desired frame orientation (ENU, NED or NWU)'), + DeclareLaunchArgument( + 'no_rotation_duration', default_value='0', + description='Duration (seconds) of no-rotation calibration'), + DeclareLaunchArgument( + 'filter_scenario', default_value='53', + description='Filter scenario: 50=general, 51=high_mag_dep, ' + '52=dynamic, 53=north_reference, 54=vru_general'), + + Node( + package='xsens_driver', + executable='mtnode.py', + name='xsens_driver', + output='screen', + respawn=True, + parameters=[{ + 'device': LaunchConfiguration('device'), + 'baudrate': LaunchConfiguration('baudrate'), + 'timeout': LaunchConfiguration('timeout'), + 'frame_id': LaunchConfiguration('frame_id'), + 'frame_local': LaunchConfiguration('frame_local'), + 'no_rotation_duration': LaunchConfiguration('no_rotation_duration'), + 'filter_scenario': LaunchConfiguration('filter_scenario'), + }], + ), + ]) diff --git a/src/xsens_driver/nodes/mtnode.py b/src/xsens_driver/nodes/mtnode.py index 0b011d78..5a4f0c43 100755 --- a/src/xsens_driver/nodes/mtnode.py +++ b/src/xsens_driver/nodes/mtnode.py @@ -1,78 +1,90 @@ -#!/usr/bin/env python -import roslib; roslib.load_manifest('xsens_driver') -import rospy +#!/usr/bin/env python3 import select +import time +import datetime +from math import radians, sqrt, atan2 + +import rclpy +from rclpy.node import Node import mtdevice import mtdef from std_msgs.msg import Header, String, UInt16 -from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, MagneticField,\ - FluidPressure, Temperature, TimeReference +from sensor_msgs.msg import (Imu, NavSatFix, NavSatStatus, MagneticField, + FluidPressure, Temperature, TimeReference) from geometry_msgs.msg import TwistStamped, PointStamped from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -import time -import datetime - -# transform Euler angles or matrix into quaternions -from math import radians, sqrt, atan2 -from tf.transformations import quaternion_from_matrix, quaternion_from_euler,\ - identity_matrix - -def get_param(name, default): - try: - v = rospy.get_param(name) - rospy.loginfo("Found parameter: %s, value: %s" % (name, str(v))) - except KeyError: - v = default - rospy.logwarn("Cannot find value for parameter: %s, assigning " - "default: %s" % (name, str(v))) - return v +try: + from tf_transformations import quaternion_from_matrix, quaternion_from_euler, identity_matrix +except ImportError: + from math import cos, sin + def quaternion_from_euler(roll, pitch, yaw): + cy, sy = cos(yaw * 0.5), sin(yaw * 0.5) + cp, sp = cos(pitch * 0.5), sin(pitch * 0.5) + cr, sr = cos(roll * 0.5), sin(roll * 0.5) + return [sr*cp*cy - cr*sp*sy, cr*sp*cy + sr*cp*sy, + cr*cp*sy - sr*sp*cy, cr*cp*cy + sr*sp*sy] + def identity_matrix(): + import numpy as np + return np.eye(4) + def quaternion_from_matrix(m): + import numpy as np + t = m[0, 0] + m[1, 1] + m[2, 2] + if t > 0: + r = sqrt(t + 1.0) + s = 0.5 / r + return [s*(m[2,1]-m[1,2]), s*(m[0,2]-m[2,0]), + s*(m[1,0]-m[0,1]), 0.5*r] + return [0., 0., 0., 1.] + + +class XSensDriver(Node): + def __init__(self): + super().__init__('xsens_driver') -class XSensDriver(object): + self.declare_parameter('device', 'auto') + self.declare_parameter('baudrate', 0) + self.declare_parameter('timeout', 0.002) + self.declare_parameter('no_rotation_duration', 0) + self.declare_parameter('frame_id', '/base_imu') + self.declare_parameter('frame_local', 'ENU') + self.declare_parameter('filter_scenario', 50) - def __init__(self): + device = self.get_parameter('device').value + baudrate = self.get_parameter('baudrate').value + timeout = self.get_parameter('timeout').value - device = get_param('~device', 'auto') - baudrate = get_param('~baudrate', 0) - timeout = get_param('~timeout', 0.002) if device == 'auto': devs = mtdevice.find_devices() if devs: device, baudrate = devs[0] - rospy.loginfo("Detected MT device on port %s @ %d bps" - % (device, baudrate)) + self.get_logger().info( + 'Detected MT device on port %s @ %d bps' % (device, baudrate)) else: - rospy.logerr("Fatal: could not find proper MT device.") - rospy.signal_shutdown("Could not find proper MT device.") - return + self.get_logger().error('Fatal: could not find proper MT device.') + raise RuntimeError('Could not find proper MT device.') if not baudrate: baudrate = mtdevice.find_baudrate(device) if not baudrate: - rospy.logerr("Fatal: could not find proper baudrate.") - rospy.signal_shutdown("Could not find proper baudrate.") - return + self.get_logger().error('Fatal: could not find proper baudrate.') + raise RuntimeError('Could not find proper baudrate.') - rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate)) + self.get_logger().info('MT node interface: %s at %d bd.' % (device, baudrate)) self.mt = mtdevice.MTDevice(device, baudrate, timeout) - # optional no rotation procedure for internal calibration of biases - # (only mark iv devices) - no_rotation_duration = get_param('~no_rotation_duration', 0) + no_rotation_duration = self.get_parameter('no_rotation_duration').value if no_rotation_duration: - rospy.loginfo("Starting the no-rotation procedure to estimate the " - "gyroscope biases for %d s. Please don't move the IMU" - " during this time." % no_rotation_duration) + self.get_logger().info( + 'Starting no-rotation procedure for %d s.' % no_rotation_duration) self.mt.SetNoRotation(no_rotation_duration) - self.frame_id = get_param('~frame_id', '/base_imu') - - self.frame_local = get_param('~frame_local', 'ENU') - - self.filter_scenario = get_param('~filter_scenario', 50) - self.mt.SetCurrentScenario(self.filter_scenario) + self.frame_id = self.get_parameter('frame_id').value + self.frame_local = self.get_parameter('frame_local').value + filter_scenario = self.get_parameter('filter_scenario').value + self.mt.SetCurrentScenario(filter_scenario) self.diag_msg = DiagnosticArray() self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1, @@ -83,38 +95,39 @@ def __init__(self): message='No status information') self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat] - # publishers created at first use to reduce topic clutter - self.diag_pub = None - self.imu_pub = None - self.gps_pub = None - self.vel_pub = None - self.mag_pub = None - self.temp_pub = None - self.press_pub = None - self.analog_in1_pub = None # decide type+header - self.analog_in2_pub = None # decide type+header - self.ecef_pub = None - self.time_ref_pub = None - # TODO pressure, ITOW from raw GPS? - self.old_bGPS = 256 # publish GPS only if new - - # publish a string version of all data; to be parsed by clients - self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10) + # Create all publishers up front (ROS 2 best practice) + self.str_pub = self.create_publisher(String, 'imu_data_str', 10) + self.imu_pub = self.create_publisher(Imu, 'imu/data', 10) + self.gps_pub = self.create_publisher(NavSatFix, 'fix', 10) + self.vel_pub = self.create_publisher(TwistStamped, 'velocity', 10) + self.mag_pub = self.create_publisher(MagneticField, 'imu/mag', 10) + self.temp_pub = self.create_publisher(Temperature, 'temperature', 10) + self.press_pub = self.create_publisher(FluidPressure, 'pressure', 10) + self.analog_in1_pub = self.create_publisher(UInt16, 'analog_in1', 10) + self.analog_in2_pub = self.create_publisher(UInt16, 'analog_in2', 10) + self.ecef_pub = self.create_publisher(PointStamped, 'ecef', 10) + self.time_ref_pub = self.create_publisher(TimeReference, 'time_reference', 10) + self.diag_pub = self.create_publisher(DiagnosticArray, '/diagnostics', 10) + + self.old_bGPS = 256 self.last_delta_q_time = None self.delta_q_rate = None + # Timer to read device at ~100 Hz + self.create_timer(0.01, self._spin_once_timer) + def reset_vars(self): self.imu_msg = Imu() - self.imu_msg.orientation_covariance = (-1., )*9 - self.imu_msg.angular_velocity_covariance = (-1., )*9 - self.imu_msg.linear_acceleration_covariance = (-1., )*9 + self.imu_msg.orientation_covariance = (-1.,) * 9 + self.imu_msg.angular_velocity_covariance = (-1.,) * 9 + self.imu_msg.linear_acceleration_covariance = (-1.,) * 9 self.pub_imu = False self.gps_msg = NavSatFix() self.pub_gps = False self.vel_msg = TwistStamped() self.pub_vel = False self.mag_msg = MagneticField() - self.mag_msg.magnetic_field_covariance = (0, )*9 + self.mag_msg.magnetic_field_covariance = (0,) * 9 self.pub_mag = False self.temp_msg = Temperature() self.temp_msg.variance = 0. @@ -130,38 +143,34 @@ def reset_vars(self): self.pub_ecef = False self.pub_diag = False - def spin(self): + def _spin_once_timer(self): try: - while not rospy.is_shutdown(): - self.spin_once() - self.reset_vars() - # Ctrl-C signal interferes with select with the ROS signal handler - # should be OSError in python 3.? + self.spin_once() + self.reset_vars() except select.error: pass def spin_once(self): - '''Read data from device and publishes ROS messages.''' - def convert_coords(x, y, z, source, dest=self.frame_local): - """Convert the coordinates between ENU, NED, and NWU.""" + """Read data from device and publish ROS 2 messages.""" + frame_local = self.frame_local + + def convert_coords(x, y, z, source, dest=frame_local): if source == dest: return x, y, z - # convert to ENU if source == 'NED': x, y, z = y, x, -z elif source == 'NWU': x, y, z = -y, x, z - # convert to desired if dest == 'NED': x, y, z = y, x, -z elif dest == 'NWU': x, y, z = y, -x, z return x, y, z - def convert_quat(q, source, dest=self.frame_local): - """Convert a quaternion between ENU, NED, and NWU.""" - def q_mult((w0, x0, y0, z0), (w1, x1, y1, z1)): - """Quaternion multiplication.""" + def convert_quat(q, source, dest=frame_local): + def q_mult(q0, q1): + w0, x0, y0, z0 = q0 + w1, x1, y1, z1 = q1 w = w0*w1 - x0*x1 - y0*y1 - z0*z1 x = w0*x1 + x0*w1 + y0*z1 - z0*y1 y = w0*y1 - x0*z1 + y0*w1 + z0*x1 @@ -173,93 +182,61 @@ def q_mult((w0, x0, y0, z0), (w1, x1, y1, z1)): q_ned_enu = (0, -1./sqrt(2), -1./sqrt(2), 0) q_nwu_enu = (1./sqrt(2), 0, 0, 1./sqrt(2)) q_nwu_ned = (0, 1, 0, 0) + if source == dest: + return q if source == 'ENU': - if dest == 'ENU': - return q - elif dest == 'NED': - return q_mult(q_enu_ned, q) - elif dest == 'NWU': - return q_mult(q_enu_nwu, q) + return q_mult(q_enu_ned, q) if dest == 'NED' else q_mult(q_enu_nwu, q) elif source == 'NED': - if dest == 'ENU': - return q_mult(q_ned_enu, q) - elif dest == 'NED': - return q - elif dest == 'NWU': - return q_mult(q_ned_nwu, q) + return q_mult(q_ned_enu, q) if dest == 'ENU' else q_mult(q_ned_nwu, q) elif source == 'NWU': - if dest == 'ENU': - return q_mult(q_nwu_enu, q) - elif dest == 'NED': - return q_mult(q_nwu_ned, q) - elif dest == 'NWU': - return q + return q_mult(q_nwu_enu, q) if dest == 'ENU' else q_mult(q_nwu_ned, q) + return q def publish_time_ref(secs, nsecs, source): - """Publish a time reference.""" - # Doesn't follow the standard publishing pattern since several time - # refs could be published simultaneously - if self.time_ref_pub is None: - self.time_ref_pub = rospy.Publisher( - 'time_reference', TimeReference, queue_size=10) time_ref_msg = TimeReference() time_ref_msg.header = self.h - time_ref_msg.time_ref.secs = secs - time_ref_msg.time_ref.nsecs = nsecs + time_ref_msg.time_ref.sec = int(secs) + time_ref_msg.time_ref.nanosec = int(nsecs) time_ref_msg.source = source self.time_ref_pub.publish(time_ref_msg) def stamp_from_itow(itow, y=None, m=None, d=None, ns=0, week=None): - """Return (secs, nsecs) from GPS time of week ms information.""" if y is not None: stamp_day = datetime.datetime(y, m, d) elif week is not None: - epoch = datetime.datetime(1980, 1, 6) # GPS epoch + epoch = datetime.datetime(1980, 1, 6) stamp_day = epoch + datetime.timedelta(weeks=week) else: - today = datetime.date.today() # using today by default - stamp_day = datetime.datetime(today.year, today.month, - today.day) - iso_day = stamp_day.isoweekday() # 1 for Monday, 7 for Sunday - # stamp for the GPS start of the week (Sunday morning) + today = datetime.date.today() + stamp_day = datetime.datetime(today.year, today.month, today.day) + iso_day = stamp_day.isoweekday() start_of_week = stamp_day - datetime.timedelta(days=iso_day) - # stamp at the millisecond precision stamp_ms = start_of_week + datetime.timedelta(milliseconds=itow) secs = time.mktime((stamp_ms.year, stamp_ms.month, stamp_ms.day, - stamp_ms.hour, stamp_ms.minute, - stamp_ms.second, 0, 0, -1)) + stamp_ms.hour, stamp_ms.minute, stamp_ms.second, + 0, 0, -1)) nsecs = stamp_ms.microsecond * 1000 + ns - if nsecs < 0: # ns can be negative + if nsecs < 0: secs -= 1 nsecs += 1e9 return (secs, nsecs) - # MTData def fill_from_RAW(raw_data): - '''Fill messages with information from 'raw' MTData block.''' - # don't publish raw imu data anymore - # TODO find what to do with that - rospy.loginfo("Got MTi data packet: 'RAW', ignored!") + self.get_logger().info("Got MTi data packet: 'RAW', ignored!") def fill_from_RAWGPS(rawgps_data): - '''Fill messages with information from 'rawgps' MTData block.''' if rawgps_data['bGPS'] < self.old_bGPS: self.pub_gps = True - # LLA - self.gps_msg.latitude = rawgps_data['LAT']*1e-7 - self.gps_msg.longitude = rawgps_data['LON']*1e-7 - self.gps_msg.altitude = rawgps_data['ALT']*1e-3 - # NED vel # TODO? + self.gps_msg.latitude = rawgps_data['LAT'] * 1e-7 + self.gps_msg.longitude = rawgps_data['LON'] * 1e-7 + self.gps_msg.altitude = rawgps_data['ALT'] * 1e-3 self.old_bGPS = rawgps_data['bGPS'] def fill_from_Temp(temp): - '''Fill messages with information from 'temperature' MTData block. - ''' self.pub_temp = True self.temp_msg.temperature = temp def fill_from_Calib(imu_data): - '''Fill messages with information from 'calibrated' MTData block.''' try: self.pub_imu = True x, y, z = convert_coords(imu_data['gyrX'], imu_data['gyrY'], @@ -268,8 +245,7 @@ def fill_from_Calib(imu_data): self.imu_msg.angular_velocity.y = y self.imu_msg.angular_velocity.z = z self.imu_msg.angular_velocity_covariance = ( - radians(0.025), 0., 0., - 0., radians(0.025), 0., + radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) self.pub_vel = True self.vel_msg.twist.angular.x = x @@ -284,9 +260,8 @@ def fill_from_Calib(imu_data): self.imu_msg.linear_acceleration.x = x self.imu_msg.linear_acceleration.y = y self.imu_msg.linear_acceleration.z = z - self.imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., - 0., 0.0004, 0., - 0., 0., 0.0004) + self.imu_msg.linear_acceleration_covariance = ( + 0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) except KeyError: pass try: @@ -300,8 +275,6 @@ def fill_from_Calib(imu_data): pass def fill_from_Orient(orient_data): - '''Fill messages with information from 'orientation' MTData block. - ''' self.pub_imu = True if 'quaternion' in orient_data: w, x, y, z = orient_data['quaternion'] @@ -310,19 +283,20 @@ def fill_from_Orient(orient_data): radians(orient_data['roll']), radians(orient_data['pitch']), radians(orient_data['yaw'])) elif 'matrix' in orient_data: + import numpy as np m = identity_matrix() m[:3, :3] = orient_data['matrix'] x, y, z, w = quaternion_from_matrix(m) + else: + return self.imu_msg.orientation.x = x self.imu_msg.orientation.y = y self.imu_msg.orientation.z = z self.imu_msg.orientation.w = w - self.imu_msg.orientation_covariance = (radians(1.), 0., 0., - 0., radians(1.), 0., - 0., 0., radians(9.)) + self.imu_msg.orientation_covariance = ( + radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) def fill_from_Auxiliary(aux_data): - '''Fill messages with information from 'Auxiliary' MTData block.''' try: self.anin1_msg.data = o['Ain_1'] self.pub_anin1 = True @@ -335,14 +309,12 @@ def fill_from_Auxiliary(aux_data): pass def fill_from_Pos(position_data): - '''Fill messages with information from 'position' MTData block.''' self.pub_gps = True self.gps_msg.latitude = position_data['Lat'] self.gps_msg.longitude = position_data['Lon'] self.gps_msg.altitude = position_data['Alt'] def fill_from_Vel(velocity_data): - '''Fill messages with information from 'velocity' MTData block.''' self.pub_vel = True x, y, z = convert_coords( velocity_data['Vel_X'], velocity_data['Vel_Y'], @@ -352,48 +324,36 @@ def fill_from_Vel(velocity_data): self.vel_msg.twist.linear.z = z def fill_from_Stat(status): - '''Fill messages with information from 'status' MTData block.''' self.pub_diag = True - if status & 0b0001: - self.stest_stat.level = DiagnosticStatus.OK - self.stest_stat.message = "Ok" - else: - self.stest_stat.level = DiagnosticStatus.ERROR - self.stest_stat.message = "Failed" - if status & 0b0010: - self.xkf_stat.level = DiagnosticStatus.OK - self.xkf_stat.message = "Valid" - else: - self.xkf_stat.level = DiagnosticStatus.WARN - self.xkf_stat.message = "Invalid" + self.stest_stat.level = (DiagnosticStatus.OK if status & 0b0001 + else DiagnosticStatus.ERROR) + self.stest_stat.message = 'Ok' if status & 0b0001 else 'Failed' + self.xkf_stat.level = (DiagnosticStatus.OK if status & 0b0010 + else DiagnosticStatus.WARN) + self.xkf_stat.message = 'Valid' if status & 0b0010 else 'Invalid' if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK - self.gps_stat.message = "Ok" + self.gps_stat.message = 'Ok' self.gps_msg.status.status = NavSatStatus.STATUS_FIX self.gps_msg.status.service = NavSatStatus.SERVICE_GPS else: self.gps_stat.level = DiagnosticStatus.WARN - self.gps_stat.message = "No fix" + self.gps_stat.message = 'No fix' self.gps_msg.status.status = NavSatStatus.STATUS_NO_FIX self.gps_msg.status.service = 0 def fill_from_Sample(ts): - '''Catch 'Sample' MTData blocks.''' - self.h.seq = ts + pass # seq was removed from Header in ROS 2 - # MTData2 def fill_from_Temperature(o): - '''Fill messages with information from 'Temperature' MTData2 block. - ''' self.pub_temp = True self.temp_msg.temperature = o['Temp'] def fill_from_Timestamp(o): - '''Fill messages with information from 'Timestamp' MTData2 block.''' try: - # put timestamp from gps UTC time if available - y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\ - o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags'] + y, m, d, hr, mi, s, ns, f = (o['Year'], o['Month'], o['Day'], + o['Hour'], o['Minute'], o['Second'], + o['ns'], o['Flags']) if f & 0x4: secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0)) publish_time_ref(secs, ns, 'UTC time') @@ -417,29 +377,27 @@ def fill_from_Timestamp(o): publish_time_ref(sample_time_coarse, 0, 'sample time coarse') except KeyError: pass - # TODO find what to do with other kind of information - pass def fill_from_Orientation_Data(o): - '''Fill messages with information from 'Orientation Data' MTData2 - block.''' self.pub_imu = True + x = y = z = w = 0. try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] except KeyError: pass try: - x, y, z, w = quaternion_from_euler(radians(o['Roll']), - radians(o['Pitch']), - radians(o['Yaw'])) + x, y, z, w = quaternion_from_euler( + radians(o['Roll']), radians(o['Pitch']), radians(o['Yaw'])) except KeyError: pass try: - a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\ - o['e'], o['f'], o['g'], o['h'], o['i'] - m = identity_matrix() - m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i)) - x, y, z, w = quaternion_from_matrix(m) + import numpy as np + a, b, c = o['a'], o['b'], o['c'] + d, e, f = o['d'], o['e'], o['f'] + g, h, i = o['g'], o['h'], o['i'] + mat = identity_matrix() + mat[:3, :3] = ((a, b, c), (d, e, f), (g, h, i)) + x, y, z, w = quaternion_from_matrix(mat) except KeyError: pass w, x, y, z = convert_quat((w, x, y, z), o['frame']) @@ -447,21 +405,16 @@ def fill_from_Orientation_Data(o): self.imu_msg.orientation.y = y self.imu_msg.orientation.z = z self.imu_msg.orientation.w = w - self.imu_msg.orientation_covariance = (radians(1.), 0., 0., - 0., radians(1.), 0., - 0., 0., radians(9.)) + self.imu_msg.orientation_covariance = ( + radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) def fill_from_Pressure(o): - '''Fill messages with information from 'Pressure' MTData2 block.''' self.press_msg.fluid_pressure = o['Pressure'] self.pub_press = True def fill_from_Acceleration(o): - '''Fill messages with information from 'Acceleration' MTData2 - block.''' self.pub_imu = True - - # FIXME not sure we should treat all in that same way + x = y = z = 0. try: x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z'] except KeyError: @@ -478,107 +431,80 @@ def fill_from_Acceleration(o): self.imu_msg.linear_acceleration.x = x self.imu_msg.linear_acceleration.y = y self.imu_msg.linear_acceleration.z = z - self.imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., - 0., 0.0004, 0., - 0., 0., 0.0004) + self.imu_msg.linear_acceleration_covariance = ( + 0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) def fill_from_Position(o): - '''Fill messages with information from 'Position' MTData2 block.''' try: self.gps_msg.latitude = o['lat'] self.gps_msg.longitude = o['lon'] self.pub_gps = True - # altMsl is deprecated - alt = o.get('altEllipsoid', o.get('altMsl', 0)) - self.gps_msg.altitude = alt + self.gps_msg.altitude = o.get('altEllipsoid', o.get('altMsl', 0)) except KeyError: pass try: - x, y, z = o['ecefX'], o['ecefY'], o['ecefZ'] - # TODO: ecef units not specified: might not be in meters! - self.ecef_msg.point.x = x - self.ecef_msg.point.y = y - self.ecef_msg.point.z = z + self.ecef_msg.point.x = o['ecefX'] + self.ecef_msg.point.y = o['ecefY'] + self.ecef_msg.point.z = o['ecefZ'] self.pub_ecef = True except KeyError: pass def fill_from_GNSS(o): - '''Fill messages with information from 'GNSS' MTData2 block.''' - try: # PVT - # time block - itow, y, m, d, ns, f = o['itow'], o['year'], o['month'],\ - o['day'], o['nano'], o['valid'] + try: + itow, y, m, d, ns, f = (o['itow'], o['year'], o['month'], + o['day'], o['nano'], o['valid']) if f & 0x4: secs, nsecs = stamp_from_itow(itow, y, m, d, ns) publish_time_ref(secs, nsecs, 'GNSS time UTC') - # flags fixtype = o['fixtype'] if fixtype == 0x00: - self.gps_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix + self.gps_msg.status.status = NavSatStatus.STATUS_NO_FIX self.gps_msg.status.service = 0 else: - self.gps_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented + self.gps_msg.status.status = NavSatStatus.STATUS_FIX self.gps_msg.status.service = NavSatStatus.SERVICE_GPS - # lat lon alt self.gps_msg.latitude = o['lat'] self.gps_msg.longitude = o['lon'] - self.gps_msg.altitude = o['height']/1e3 + self.gps_msg.altitude = o['height'] / 1e3 self.pub_gps = True - # TODO velocity? - # TODO 2D heading? - # TODO DOP? except KeyError: pass - # TODO publish Sat Info def fill_from_Angular_Velocity(o): - '''Fill messages with information from 'Angular Velocity' MTData2 - block.''' try: dqw, dqx, dqy, dqz = convert_quat( - (o['Delta q0'], o['Delta q1'], o['Delta q2'], - o['Delta q3']), + (o['Delta q0'], o['Delta q1'], o['Delta q2'], o['Delta q3']), o['frame']) - now = rospy.Time.now() + now_ns = self.get_clock().now().nanoseconds if self.last_delta_q_time is None: - self.last_delta_q_time = now + self.last_delta_q_time = now_ns else: - # update rate (filtering needed to account for lag variance) - delta_t = (now - self.last_delta_q_time).to_sec() + delta_t = (now_ns - self.last_delta_q_time) * 1e-9 if self.delta_q_rate is None: - self.delta_q_rate = 1./delta_t - delta_t_filtered = .95/self.delta_q_rate + .05*delta_t - # rate in necessarily integer - self.delta_q_rate = round(1./delta_t_filtered) - #print(delta_t, delta_t_filtered, self.delta_q_rate) - self.last_delta_q_time = now - # relationship between \Delta q and velocity \bm{\omega}: - # \bm{w} = \Delta t . \bm{\omega} - # \theta = |\bm{w}| - # \Delta q = [cos{\theta/2}, sin{\theta/2)/\theta . \bm{\omega} - # extract rotation angle over delta_t + self.delta_q_rate = 1. / delta_t + delta_t_filtered = .95 / self.delta_q_rate + .05 * delta_t + self.delta_q_rate = round(1. / delta_t_filtered) + self.last_delta_q_time = now_ns ca_2, sa_2 = dqw, sqrt(dqx**2 + dqy**2 + dqz**2) ca = ca_2**2 - sa_2**2 - sa = 2*ca_2*sa_2 + sa = 2 * ca_2 * sa_2 rotation_angle = atan2(sa, ca) - # compute rotation velocity rotation_speed = rotation_angle * self.delta_q_rate - f = rotation_speed / sa_2 - x, y, z = f*dqx, f*dqy, f*dqz - self.imu_msg.angular_velocity.x = x - self.imu_msg.angular_velocity.y = y - self.imu_msg.angular_velocity.z = z - self.imu_msg.angular_velocity_covariance = ( - radians(0.025), 0., 0., - 0., radians(0.025), 0., - 0., 0., radians(0.025)) - self.pub_imu = True - self.vel_msg.twist.angular.x = x - self.vel_msg.twist.angular.y = y - self.vel_msg.twist.angular.z = z - self.pub_vel = True - #print(x, y, z) + if sa_2 != 0: + f = rotation_speed / sa_2 + x, y, z = f*dqx, f*dqy, f*dqz + self.imu_msg.angular_velocity.x = x + self.imu_msg.angular_velocity.y = y + self.imu_msg.angular_velocity.z = z + self.imu_msg.angular_velocity_covariance = ( + radians(0.025), 0., 0., 0., radians(0.025), 0., + 0., 0., radians(0.025)) + self.pub_imu = True + self.vel_msg.twist.angular.x = x + self.vel_msg.twist.angular.y = y + self.vel_msg.twist.angular.z = z + self.pub_vel = True except KeyError: pass try: @@ -588,58 +514,45 @@ def fill_from_Angular_Velocity(o): self.imu_msg.angular_velocity.y = y self.imu_msg.angular_velocity.z = z self.imu_msg.angular_velocity_covariance = ( - radians(0.025), 0., 0., - 0., radians(0.025), 0., + radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) self.pub_imu = True self.vel_msg.twist.angular.x = x self.vel_msg.twist.angular.y = y self.vel_msg.twist.angular.z = z self.pub_vel = True - #print(x, y, z) - #print except KeyError: pass def fill_from_GPS(o): - '''Fill messages with information from 'GPS' MTData2 block.''' - # TODO DOP - try: # SOL - x, y, z = o['ecefX'], o['ecefY'], o['ecefZ'] - self.ecef_msg.point.x = x * 0.01 # data is in cm - self.ecef_msg.point.y = y * 0.01 - self.ecef_msg.point.z = z * 0.01 + try: + self.ecef_msg.point.x = o['ecefX'] * 0.01 + self.ecef_msg.point.y = o['ecefY'] * 0.01 + self.ecef_msg.point.z = o['ecefZ'] * 0.01 self.pub_ecef = True - vx, vy, vz = o['ecefVX'], o['ecefVY'], o['ecefVZ'] - self.vel_msg.twist.linear.x = vx * 0.01 # data is in cm - self.vel_msg.twist.linear.y = vy * 0.01 - self.vel_msg.twist.linear.z = vz * 0.01 + self.vel_msg.twist.linear.x = o['ecefVX'] * 0.01 + self.vel_msg.twist.linear.y = o['ecefVY'] * 0.01 + self.vel_msg.twist.linear.z = o['ecefVZ'] * 0.01 self.pub_vel = True itow, ns, week, f = o['iTOW'], o['fTOW'], o['Week'], o['Flags'] if (f & 0x0C) == 0xC: secs, nsecs = stamp_from_itow(itow, ns=ns, week=week) publish_time_ref(secs, nsecs, 'GPS Time') - # TODO there are other pieces of information that we could - # publish except KeyError: pass - try: # Time UTC - itow, y, m, d, ns, f = o['iTOW'], o['year'], o['month'],\ - o['day'], o['nano'], o['valid'] + try: + itow, y, m, d, ns, f = (o['iTOW'], o['year'], o['month'], + o['day'], o['nano'], o['valid']) if f & 0x4: secs, nsecs = stamp_from_itow(itow, y, m, d, ns) publish_time_ref(secs, nsecs, 'GPS Time UTC') except KeyError: pass - # TODO publish SV Info def fill_from_SCR(o): - '''Fill messages with information from 'SCR' MTData2 block.''' - # TODO that's raw information pass def fill_from_Analog_In(o): - '''Fill messages with information from 'Analog In' MTData2 block.''' try: self.anin1_msg.data = o['analogIn1'] self.pub_anin1 = True @@ -652,7 +565,6 @@ def fill_from_Analog_In(o): pass def fill_from_Magnetic(o): - '''Fill messages with information from 'Magnetic' MTData2 block.''' x, y, z = convert_coords(o['magX'], o['magY'], o['magZ'], o['frame']) self.mag_msg.magnetic_field.x = x @@ -661,7 +573,6 @@ def fill_from_Magnetic(o): self.pub_mag = True def fill_from_Velocity(o): - '''Fill messages with information from 'Velocity' MTData2 block.''' x, y, z = convert_coords(o['velX'], o['velY'], o['velZ'], o['frame']) self.vel_msg.twist.linear.x = x @@ -670,109 +581,84 @@ def fill_from_Velocity(o): self.pub_vel = True def fill_from_Status(o): - '''Fill messages with information from 'Status' MTData2 block.''' try: - status = o['StatusByte'] - fill_from_Stat(status) + fill_from_Stat(o['StatusByte']) except KeyError: pass try: - status = o['StatusWord'] - fill_from_Stat(status) + fill_from_Stat(o['StatusWord']) except KeyError: pass def find_handler_name(name): - return "fill_from_%s" % (name.replace(" ", "_")) + return 'fill_from_%s' % name.replace(' ', '_') - # get data try: data = self.mt.read_measurement() except mtdef.MTTimeoutException: time.sleep(0.1) return - # common header + self.h = Header() - self.h.stamp = rospy.Time.now() + self.h.stamp = self.get_clock().now().to_msg() self.h.frame_id = self.frame_id - # set default values self.reset_vars() - # fill messages based on available data fields for n, o in data.items(): try: locals()[find_handler_name(n)](o) except KeyError: - rospy.logwarn("Unknown MTi data packet: '%s', ignoring." % n) + self.get_logger().warn( + "Unknown MTi data packet: '%s', ignoring." % n) - # publish available information if self.pub_imu: self.imu_msg.header = self.h - if self.imu_pub is None: - self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10) self.imu_pub.publish(self.imu_msg) if self.pub_gps: self.gps_msg.header = self.h - if self.gps_pub is None: - self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10) self.gps_pub.publish(self.gps_msg) if self.pub_vel: self.vel_msg.header = self.h - if self.vel_pub is None: - self.vel_pub = rospy.Publisher('velocity', TwistStamped, - queue_size=10) self.vel_pub.publish(self.vel_msg) if self.pub_mag: self.mag_msg.header = self.h - if self.mag_pub is None: - self.mag_pub = rospy.Publisher('imu/mag', MagneticField, - queue_size=10) self.mag_pub.publish(self.mag_msg) if self.pub_temp: self.temp_msg.header = self.h - if self.temp_pub is None: - self.temp_pub = rospy.Publisher('temperature', Temperature, - queue_size=10) self.temp_pub.publish(self.temp_msg) if self.pub_press: self.press_msg.header = self.h - if self.press_pub is None: - self.press_pub = rospy.Publisher('pressure', FluidPressure, - queue_size=10) self.press_pub.publish(self.press_msg) if self.pub_anin1: - if self.analog_in1_pub is None: - self.analog_in1_pub = rospy.Publisher('analog_in1', - UInt16, queue_size=10) self.analog_in1_pub.publish(self.anin1_msg) if self.pub_anin2: - if self.analog_in2_pub is None: - self.analog_in2_pub = rospy.Publisher('analog_in2', UInt16, - queue_size=10) self.analog_in2_pub.publish(self.anin2_msg) if self.pub_ecef: self.ecef_msg.header = self.h - if self.ecef_pub is None: - self.ecef_pub = rospy.Publisher('ecef', PointStamped, - queue_size=10) self.ecef_pub.publish(self.ecef_msg) if self.pub_diag: self.diag_msg.header = self.h - if self.diag_pub is None: - self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, - queue_size=10) self.diag_pub.publish(self.diag_msg) - # publish string representation - self.str_pub.publish(str(data)) + + str_msg = String() + str_msg.data = str(data) + self.str_pub.publish(str_msg) -def main(): - '''Create a ROS node and instantiate the class.''' - rospy.init_node('xsens_driver') - driver = XSensDriver() - driver.spin() +def main(args=None): + """Create a ROS 2 node and start the XSens driver.""" + rclpy.init(args=args) + node = XSensDriver() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': main() + diff --git a/src/xsens_driver/package.xml b/src/xsens_driver/package.xml index 22f8ffca..34a8967c 100644 --- a/src/xsens_driver/package.xml +++ b/src/xsens_driver/package.xml @@ -1,27 +1,29 @@ - + + + xsens_driver 2.1.0 - - ROS Driver for XSens MT/MTi/MTi-G devices. - + ROS 2 Driver for XSens MT/MTi/MTi-G devices. Francis Colas BSD - catkin + ament_cmake + ament_cmake_python - rospy - std_msgs - tf - sensor_msgs - geometry_msgs - diagnostic_msgs + rclpy + std_msgs + tf2_ros + sensor_msgs + geometry_msgs + diagnostic_msgs - rospy - std_msgs - tf - sensor_msgs - geometry_msgs - diagnostic_msgs + ament_lint_auto + ament_lint_common + pytest + + + ament_cmake + diff --git a/src/xsens_driver/resource/xsens_driver b/src/xsens_driver/resource/xsens_driver new file mode 100644 index 00000000..e69de29b