Merge pull request #1 from SFU-MARS/humble

add parameters to node

.github/actions/lint/action.yml

@@ -0,0 +1,6 @@
+name: 'Lint'
+description: 'Lint using devcontainer'
+runs:
+  using: 'docker'
+  image: '../../../.devcontainer/Dockerfile'
+  entrypoint: ".github/actions/lint/run.sh"

.github/actions/lint/run.sh

@@ -0,0 +1,5 @@
+#!/bin/bash
+set -e
+
+./setup.sh
+ament_${LINTER} src/

.github/actions/test/action.yml

@@ -0,0 +1,6 @@
+name: 'Test'
+description: 'Test using devcontainer'
+runs:
+  using: 'docker'
+  image: '../../../.devcontainer/Dockerfile'
+  entrypoint: ".github/actions/test/run.sh"

.github/actions/test/run.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+set -e
+
+./setup.sh
+./build.sh
+./test.sh

.github/dependabot.yml

@@ -0,0 +1,7 @@
+version: 2
+updates:
+- package-ecosystem: github-actions
+  directory: "/"
+  schedule:
+    interval: daily
+  open-pull-requests-limit: 10

.github/workflows/docs.yaml

@@ -0,0 +1,17 @@
+name: docs
+
+on:
+  release:
+  push:
+    tags:
+      - 'v*'
+  workflow_dispatch:
+
+jobs:
+  publish:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v4
+      - name: Build and push docs
+        uses: athackst/[email protected]

.github/workflows/ros.yaml

@@ -0,0 +1,49 @@
+name: ROS
+
+on:
+  pull_request:
+  push:
+    branches: 
+      - humble*
+      - iron*
+      - rolling*
+  workflow_dispatch:
+
+jobs:
+  test:
+    name: test
+    runs-on: ubuntu-latest
+    steps:
+      - 
+        name: Checkout code
+        uses: actions/checkout@v4
+      -
+        name: Test
+        uses: ./.github/actions/test/
+
+  lint:
+    name: ament_${{ matrix.linter }}
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+          linter: [cppcheck, cpplint, uncrustify, lint_cmake, xmllint, flake8, pep257]
+    steps:
+      - 
+        name: Checkout code
+        uses: actions/checkout@v4
+      -
+        name: Run linter
+        uses: ./.github/actions/lint/
+        env: 
+          LINTER: ${{ matrix.linter }}
+
+  complete:
+    name: Tests passed
+    needs:
+      - lint
+      - test
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check
+        run: echo "Completed successfully!"

.vscode/settings.json

@@ -9,11 +9,13 @@
         "*.xacro": "xml"
     },
     "python.analysis.extraPaths": [
-        "/opt/ros/humble/lib/python3.10/site-packages/"
+        "/opt/ros/humble/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages"
     ],
     // Autocomplete from ros python packages
     "python.autoComplete.extraPaths": [
-        "/opt/ros/humble/lib/python3.10/site-packages/"
+        "/opt/ros/humble/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages"
     ],
     // Environment file lets vscode find python files within workspace
     "python.envFile": "${workspaceFolder}/.env",

README.md

@@ -58,4 +58,4 @@ Rosbags are used to record ROS data.
   - A default file name will be used if the `-o` option is not used
   - **Warning**: Using `ros2 bag record --all` will usually result in _gigantic_ file sizes
 3. `ros2 run plotjuggler plotjuggler`
-4. Load data or stream from current topics
+4. Load data or stream from current topics

src/controller/controller/controller.py

@@ -3,6 +3,30 @@
 from geometry_msgs.msg import PoseStamped, Twist, Vector3
 import numpy as np
 
+class ControllerNode(Node):
+    def __init__(self) -> None:
+        super().__init__('robot_controller')
+        self.sub = self.create_subscription(PoseStamped, '/robot_pose', self.send_ctrl, 10)
+        self.pub = self.create_publisher(Twist, '/cmd_vel', 1)
+
+        self.declare_parameter('algorithm', 'DQN')
+        algo: str = self.get_parameter('algorithm').get_parameter_value().string_value
+        self.get_logger().info(f"Using algorithm {algo}")
+
+    def send_ctrl(self, msg: PoseStamped):
+        robot_x = msg.pose.position.x
+        robot_y = msg.pose.position.y
+        _, _, robot_theta = euler_from_quaternion(msg.pose.orientation)
+        self.get_logger().info(f'Robot position: x={robot_x:0.2f} y={robot_y:0.2f} theta={robot_theta:0.2f}')
+
+        # Do research code 
+
+        twist_msg = Twist()
+        twist_msg.linear = Vector3(x=1.0, y=0.0, z=0.0)
+        twist_msg.angular = Vector3(x=0.0, y=0.0, z=0.0)
+
+        self.pub.publish(twist_msg)
+
 def euler_from_quaternion(quaternion):
     """
     Converts quaternion (w in last place) to euler roll, pitch, yaw
@@ -27,25 +51,6 @@ def euler_from_quaternion(quaternion):
 
     return roll, pitch, yaw
 
-class ControllerNode(Node):
-    def __init__(self) -> None:
-        super().__init__('robot_controller')
-        self.sub = self.create_subscription(PoseStamped, '/robot_pose', self.send_ctrl, 10)
-        self.pub = self.create_publisher(Twist, '/cmd_vel', 1)
-
-    def send_ctrl(self, msg: PoseStamped):
-        robot_x = msg.pose.position.x
-        robot_y = msg.pose.position.y
-        _, _, robot_theta = euler_from_quaternion(msg.pose.orientation)
-        self.get_logger().info(f'Robot position: x={robot_x:0.2f} y={robot_y:0.2f} theta={robot_theta:0.2f}')
-
-        # Do research code 
-
-        twist_msg = Twist()
-        twist_msg.linear = Vector3(x=1.0, y=0.0, z=0.0)
-        twist_msg.angular = Vector3(x=0.0, y=0.0, z=0.0)
-
-        self.pub.publish(twist_msg)
 
 def main(args=None):
     rclpy.init(args=args)

src/controller/launch/experiment.launch.py

@@ -1,13 +1,21 @@
+<<<<<<< HEAD
+import os
+from ament_index_python import get_package_share_directory
+=======
+>>>>>>> origin/main
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
 def generate_launch_description():
     ld = LaunchDescription()
 
+    config = os.path.join(get_package_share_directory('controller'), 'param', 'robot_params.yaml')
     controller_node = Node(
         package="controller",
         executable="robot_controller",
+        parameters = [config]
     )
+
     sim_node = Node(
         package="vicon_simulator",
         executable="robot_pose_simulator",

src/controller/param/robot_params.yaml

@@ -0,0 +1,3 @@
+robot_controller:
+  ros__parameters:
+    algorithm: "LQR"

src/controller/setup.py

@@ -12,8 +12,8 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name), glob('launch/*.launch.py'))
-
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+        (os.path.join('share', package_name, 'param'), glob('param/*.yaml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,