Merge pull request #13 from mleegwt/feature/workaround

Testing of navigation

Author: mleegwt

README.md

@@ -3,6 +3,9 @@
 Load ROS 2:
 `source /opt/ros/jazzy/setup.bash`
 
+Navigate to ROS workspace:
+`cd ros_ws`
+
 Build and run temperature sensor node (my SensorID):
 `colcon build && source ./install/setup.bash && ros2 run tempreader tempreaderNode --ros-args -p sensorId:=28.C23646D48524 &`
 

ros_ws/src/boat_interfaces/CMakeLists.txt

@@ -10,10 +10,11 @@ find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/BoatHeading.msg"
+  "msg/Waypoints.msg"
+  DEPENDENCIES geometry_msgs
 )
 
 if(BUILD_TESTING)

ros_ws/src/boat_interfaces/msg/BoatHeading.msg

@@ -0,0 +1,2 @@
+# Inspired by: https://docs.ros.org/en/iron/p/nav2_msgs/interfaces/action/FollowGPSWaypoints.html
+geometry_msgs/PoseStamped[] gps_poses

ros_ws/src/boat_interfaces/msg/Waypoints.msg

@@ -7,15 +7,16 @@
   <maintainer email="mleegwt@users.noreply.github.com">pi</maintainer>
   <license>TODO: License declaration</license>
 
+  <build_depend>geometry_msgs</build_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>
 
-  <!--  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>-->
-
   <export>
     <build_type>ament_cmake</build_type>
   </export>

ros_ws/src/boat_interfaces/package.xml

@@ -4,6 +4,7 @@
 import math
 import rclpy
 from rclpy.node import Node
+from std_msgs.msg import Float64
 
 # Initialize serial communication for actuator control
 serial_port = "/dev/ttyUSB0"  # Update with your port
@@ -60,9 +61,9 @@ class BoatControlNode(Node):
 
     def __init__(self):
         super().__init__('boat_control_node')
-        self.compass_pub = self.create_publisher(BoatHeading, '/compass', 10)
-        self.heading_sub = self.create_subscriber(Float64, '/desired_heading', self.heading_callback, 10)
-        self.speed_sub = self.create_subscriber(Float64, '/desired_speed', self.speed_callback, 10)
+        self.compass_pub = self.create_publisher(Float64, '/compass', 10)
+        self.heading_sub = self.create_subscription(Float64, '/desired_heading', self.heading_callback, 10)
+        self.speed_sub = self.create_subscription(Float64, '/desired_speed', self.speed_callback, 10)
         self.current_heading = None
         timer_period = 1  # Seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
@@ -72,8 +73,8 @@ def timer_callback(self):
         current_heading = get_heading_command()
         current_heading = get_heading_command()# dual reading to flush !
 
-        msg = BoatHeading()
-        msg.heading = current_heading
+        msg = Float64()
+        msg.data = current_heading
         self.compass_pub.publish(msg)
 
     def heading_callback(self, heading):
@@ -94,9 +95,9 @@ def main():
     try:
         time.sleep(0.5)
         response = ser.readline().decode().strip()  # Read response from the actuator
-        print(f"Received: {response}")
+        print(f"Received: {response}\n")
         response = ser.readline().decode().strip()  # Read response from the actuator
-        print(f"Received: {response}")
+        print(f"Received: {response}\n")
         send_enable_command()
         time.sleep(1)
         send_calib_command()

ros_ws/src/boatcontrol/boatcontrol/boatcontrolnode.py

@@ -0,0 +1,18 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import launch_testing.actions
+
+def generate_launch_description():
+    nodes = [
+        Node(
+            package='boatcontrol',
+            executable='boatcontrol_node',
+            name='boatcontrol_node',
+            output='screen',
+            parameters=[{'use_sim_time': False}]
+        )
+    ]
+
+    return LaunchDescription(nodes + [
+        launch_testing.actions.ReadyToTest()
+    ])

ros_ws/src/boatcontrol/launch/boatcontrol.launch.py

@@ -11,6 +11,12 @@
   <depend>rclpy</depend>
   <depend>serial</depend>
   <depend>time</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend>boat_interfaces</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_testing</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_pep257</test_depend>

ros_ws/src/boatcontrol/package.xml

@@ -7,8 +7,8 @@
     version='0.0.0',
     packages=find_packages(exclude=['test']),
     data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name + '/launch', ['launch/boatcontrol.launch.py']),
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
@@ -20,6 +20,7 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'boatcontrol_node = boatcontrol.boatcontrolnode:main'
         ],
     },
 )

ros_ws/src/boatcontrol/setup.py

@@ -0,0 +1,42 @@
+from launch import LaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
+
+def generate_launch_description():
+    gpsd_client_dir = get_package_share_directory('gpsd_client')
+    gpsd_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(gpsd_client_dir, 'launch', 'gpsd_client-launch.py')
+        )
+    )
+
+    boatcontrol_dir = get_package_share_directory('boatcontrol')
+    boatcontrol = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(boatcontrol_dir, 'launch', 'boatcontrol.launch.py')
+        )
+    )
+
+    sensorfusion_dir = get_package_share_directory('sensorfusion')
+    sensorfusion = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(sensorfusion_dir, 'launch', 'sensorfusion.launch.py')
+        )
+    )
+
+    nav_dir = get_package_share_directory('navigation')
+    nav_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav_dir, 'launch', 'nav2_stack.launch.py')
+        )
+    )
+
+    return LaunchDescription([
+        gpsd_launch,
+        boatcontrol,
+        sensorfusion,
+        nav_launch
+    ])