Merge pull request #9 from mleegwt/master

Sensor fusion: Merge GPS fix and compass

Author: mleegwt

.github/workflows/action.yml

@@ -10,8 +10,16 @@ jobs:
       - uses: ros-tooling/setup-ros@v0.7
         with:
           required-ros-distributions: jazzy
+      - uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          package-name: boat_interfaces
+          target-ros2-distro: jazzy
       - uses: ros-tooling/action-ros-ci@v0.3
         with:
           package-name: tempreader
           target-ros2-distro: jazzy
+      - uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          package-name: sensorfusion
+          target-ros2-distro: jazzy
 

README.md

@@ -1 +1,18 @@
-# karaburan
+# karaburan
+
+Load ROS 2:
+`source /opt/ros/jazzy/setup.bash`
+
+Build and run temperature sensor node (my SensorID):
+`colcon build && source ./install/setup.bash && ros2 run tempreader tempreaderNode --ros-args -p sensorId:=28.C23646D48524 &`
+
+Launch GPSD client node:
+`ros2 launch gpsd_client gpsd_client-launch.py &`
+
+Install ros-jazzy-gpsd-client
+
+Read fixes from GPS:
+`ros2 topic echo /fix`
+
+Read temperature:
+`ros2 topic echo /temperature`

ros_ws/src/boat_interfaces/CMakeLists.txt

@@ -0,0 +1,31 @@
+cmake_minimum_required(VERSION 3.8)
+project(boat_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/BoatHeading.msg"
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

ros_ws/src/boat_interfaces/msg/BoatHeading.msg

@@ -0,0 +1 @@
+uint32 heading

ros_ws/src/boat_interfaces/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>boat_interfaces</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="mleegwt@users.noreply.github.com">pi</maintainer>
+  <license>TODO: License declaration</license>
+
+  <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>
+</package>

ros_ws/src/sensorfusion/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sensorfusion</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="mleegwt@users.noreply.github.com">pi</maintainer>
+  <license>TODO: License declaration</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>boat_interfaces</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros_ws/src/sensorfusion/resource/sensorfusion

@@ -0,0 +1,71 @@
+import math
+
+from boat_interfaces.msg import BoatHeading
+import rclpy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from geometry_msgs.msg import Quaternion
+from rclpy.node import Node
+from sensor_msgs.msg import NavSatFix
+
+
+def heading_to_quaternion(heading_deg):
+    # Convert compass heading from degrees to radians
+    heading_rad = math.radians(heading_deg)
+
+    # Calculate quaternion components
+    qx = 0.0
+    qy = 0.0
+    qz = math.sin(heading_rad / 2)
+    qw = math.cos(heading_rad / 2)
+
+    # Return quaternion
+    return Quaternion(x=qx, y=qy, z=qz, w=qw)
+
+
+class SensorFusionNode(Node):
+
+    def __init__(self):
+        super().__init__('sensor_fusion_node')
+        self.gps_sub = self.create_subscription(NavSatFix, '/fix', self.gps_callback, 10)
+        self.compass_sub = self.create_subscription(BoatHeading, '/compass', self.compass_callback, 10)
+        self.pose_pub = self.create_publisher(PoseWithCovarianceStamped, '/amcl_pose', 10)
+        self.current_lat = None
+        self.current_lon = None
+        self.current_heading = None  # Heading from your custom compass
+
+    def gps_callback(self, msg):
+        self.current_lat = msg.latitude
+        self.current_lon = msg.longitude
+        self.publish_pose()
+
+    def compass_callback(self, heading):
+        self.current_heading = heading
+        self.publish_pose()
+
+    def publish_pose(self):
+        if self.current_lat is not None and self.current_lon is not None and self.current_heading is not None:
+            pose_msg = PoseWithCovarianceStamped()
+            pose_msg.header.stamp = self.get_clock().now().to_msg()
+            pose_msg.header.frame_id = 'odom'
+
+            # Convert GPS to XY coordinates (this will need to be done with a transform library)
+            # Simplified; typically requires a transform
+            pose_msg.pose.pose.position.x = self.current_lon  
+            pose_msg.pose.pose.position.y = self.current_lat
+
+            # Convert compass heading to quaternion for orientation
+            pose_msg.pose.pose.orientation = heading_to_quaternion(self.current_heading.heading)
+
+            self.pose_pub.publish(pose_msg)
+
+
+def main():
+    rclpy.init()
+    node = SensorFusionNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

ros_ws/src/sensorfusion/sensorfusion/__init__.py

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/sensorfusion
+[install]
+install_scripts=$base/lib/sensorfusion