Marc/humble carto

.gitignore

@@ -47,3 +47,5 @@ qtcreator-*
 
 # Catkin custom files
 CATKIN_IGNORE
+
+/gazebo/.vscode

README.md

@@ -1 +1 @@
-# TRAILBot
+# TRAILBot

base/launch/base.launch.py

@@ -0,0 +1,97 @@
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare("trailbot_gazebo"), "urdf", "husky.urdf.xacro"]
+            ),
+            " ",
+            "name:=husky",
+            " ",
+            "prefix:=''",
+            " ",
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    config_husky_velocity_controller = PathJoinSubstitution(
+        [FindPackageShare("husky_control"),
+        "config",
+        "control.yaml"],
+    )
+
+    node_robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="screen",
+        parameters=[robot_description],
+    )
+
+    node_controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, config_husky_velocity_controller],
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+    )
+
+    spawn_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+        output="screen",
+    )
+
+    spawn_husky_velocity_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["husky_velocity_controller"],
+        output="screen",
+    )
+
+    # Launch husky_control/control.launch.py which is just robot_localization.
+    launch_husky_control = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution(
+        [FindPackageShare("husky_control"), 'launch', 'control.launch.py'])))
+
+    # Launch husky_control/teleop_base.launch.py which is various ways to tele-op
+    # the robot but does not include the joystick. Also, has a twist mux.
+    launch_husky_teleop_base = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution(
+        [FindPackageShare("husky_control"), 'launch', 'teleop_base.launch.py'])))
+
+    # Launch husky_control/teleop_joy.launch.py which is tele-operation using a physical joystick.
+    launch_husky_teleop_joy = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution(
+        [FindPackageShare("husky_control"), 'launch', 'teleop_joy.launch.py'])))
+
+
+    # Launch husky_bringup/accessories.launch.py which is the sensors commonly used on the Husky.
+    launch_husky_accessories = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution(
+        [FindPackageShare("husky_bringup"), 'launch', 'accessories.launch.py'])))
+
+
+    ld = LaunchDescription()
+    ld.add_action(node_robot_state_publisher)
+    ld.add_action(node_controller_manager)
+    ld.add_action(spawn_controller)
+    ld.add_action(spawn_husky_velocity_controller)
+    ld.add_action(launch_husky_control)
+    ld.add_action(launch_husky_teleop_base)
+    ld.add_action(launch_husky_teleop_joy)
+    ld.add_action(launch_husky_accessories)
+
+    return ld

fsm/.vscode/c_cpp_properties.json

@@ -0,0 +1,30 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/marc/test_ncrn/install/velodyne_gazebo_plugins/include/**",
+        "/home/marc/test_ncrn/install/pointcloud_to_laserscan/include/**",
+        "/home/marc/test_ncrn/install/lio_sam/include/**",
+        "/opt/ros/humble/include/**",
+        "/home/marc/test_ncrn/src/LIO-SAM/include/**",
+        "/home/marc/test_ncrn/src/TRAILBot/control/include/**",
+        "/home/marc/test_ncrn/src/TRAILBot/perception/include/**",
+        "/home/marc/test_ncrn/src/TRAILBot/planning/include/**",
+        "/home/marc/test_ncrn/src/nature-stack/include/**",
+        "/home/marc/test_ncrn/src/pointcloud_to_laserscan/include/**",
+        "/home/marc/test_ncrn/src/velodyne_simulator/velodyne_gazebo_plugins/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

fsm/.vscode/settings.json

@@ -0,0 +1,19 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/marc/test_ncrn/install/fsm/lib/python3.10/site-packages",
+        "/home/marc/test_ncrn/build/my_package",
+        "/home/marc/test_ncrn/install/my_package/lib/python3.10/site-packages",
+        "/home/marc/test_ncrn/install/lio_sam/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/marc/test_ncrn/install/fsm/lib/python3.10/site-packages",
+        "/home/marc/test_ncrn/build/my_package",
+        "/home/marc/test_ncrn/install/my_package/lib/python3.10/site-packages",
+        "/home/marc/test_ncrn/install/lio_sam/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "ros.distro": "humble"
+}

fsm/README.md

@@ -0,0 +1,51 @@
+# clone
+    $ cd ~/ros2_ws/src
+    $ git clone https://github.com/uleroboticsgroup/simple_node.git
+    $ git clone https://github.com/uleroboticsgroup/yasmin.git
+
+# dependencies
+    $ cd yasmin
+    $ pip3 install -r requirements.txt
+
+# colcon
+    $ cd ~/ros2_ws
+    $ colcon build
+
+# add fsm node and navigator node to launch directory
+From the marc/humble_carto branch add the fsm folder into the trailbot's launch directory
+
+# modify launch file
+Add the following to the launch file
+
+    # FSM Node
+    fsm_node = Node(
+        package='fsm',
+        executable='trailbot_fsm',
+        name='fsm',
+        output='screen'
+    )
+
+    # Navigator Node
+    navigator_node = Node(
+        package='fsm',
+        executable='navigator_node',
+        name='test_cmd_vel_node',
+        output='screen'
+    )
+
+    ld.add_action(fsm_node)
+    ld.add_action(navigator_node)
+
+# modify the subscriber in the trailbot_fsm and navigator_node to subscribe to the perception and voice assistant node
+
+# FYI
+The fsm accepts PoseStamped msgs for its goal position i.e 
+
+    ros2 topic pub --once /target_location geometry_msgs/msg/PoseStamped '{header: {frame_id: "map"}, pose: {position: {x: 3.0, y: -3.0, z: 0.002472}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}'
+
+
+
+
+
+
+

fsm/fsm/.vscode/c_cpp_properties.json

@@ -0,0 +1,38 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/daniel/Documents/S2023/install/yasmin_viewer/include/**",
+        "/home/daniel/Documents/S2023/install/yasmin_ros/include/**",
+        "/home/daniel/Documents/S2023/install/yasmin_msgs/include/**",
+        "/home/daniel/Documents/S2023/install/yasmin/include/**",
+        "/home/daniel/Documents/S2023/install/simple_node/include/**",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_viewer/include/**",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_ros/include/**",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_msgs/include/**",
+        "/home/daniel/Documents/ros2_ws/install/yasmin/include/**",
+        "/home/daniel/Documents/ros2_ws/install/simple_node/include/**",
+        "/home/daniel/Documents/ros2_ws/install/action_tutorials_interfaces/include/**",
+        "/opt/ros/humble/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/control/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/perception/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/planning/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/simple_node/simple_node/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/yasmin/yasmin/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/yasmin/yasmin_ros/include/**",
+        "/home/daniel/Documents/S2023/src/TRAILBot/yasmin/yasmin_viewer/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

fsm/fsm/.vscode/settings.json

@@ -0,0 +1,48 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/daniel/Documents/S2023/install/yasmin_demo/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin_viewer/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin_ros/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin_msgs/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/simple_node/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/build/fsm_node",
+        "/home/daniel/Documents/S2023/install/fsm_node/lib/python3.10/site-packages",
+        "/home/daniel/Documents/S2023/build/fsm",
+        "/home/daniel/Documents/S2023/install/fsm/lib/python3.10/site-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_demo/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_viewer/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_ros/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_msgs/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/simple_node/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/build/node_tutorial",
+        "/home/daniel/Documents/ros2_ws/install/node_tutorial/lib/python3.10/site-packages",
+        "/home/daniel/Documents/ros2_ws/install/action_tutorials_interfaces/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/daniel/Documents/S2023/install/yasmin_demo/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin_viewer/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin_ros/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin_msgs/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/yasmin/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/install/simple_node/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/S2023/build/fsm_node",
+        "/home/daniel/Documents/S2023/install/fsm_node/lib/python3.10/site-packages",
+        "/home/daniel/Documents/S2023/build/fsm",
+        "/home/daniel/Documents/S2023/install/fsm/lib/python3.10/site-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_demo/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_viewer/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_ros/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin_msgs/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/yasmin/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/install/simple_node/local/lib/python3.10/dist-packages",
+        "/home/daniel/Documents/ros2_ws/build/node_tutorial",
+        "/home/daniel/Documents/ros2_ws/install/node_tutorial/lib/python3.10/site-packages",
+        "/home/daniel/Documents/ros2_ws/install/action_tutorials_interfaces/local/lib/python3.10/dist-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}