diff --git a/src/dcaiti_control b/src/dcaiti_control deleted file mode 160000 index d978233..0000000 --- a/src/dcaiti_control +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d97823342390b8de78b2710dff6c557596e718bf diff --git a/src/dcaiti_control/CMakeLists.txt b/src/dcaiti_control/CMakeLists.txt new file mode 100644 index 0000000..abe6f7e --- /dev/null +++ b/src/dcaiti_control/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.5) +project(dcaiti_control) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + DIRECTORY config description launch worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/dcaiti_control/LICENSE.md b/src/dcaiti_control/LICENSE.md new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/src/dcaiti_control/LICENSE.md @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/dcaiti_control/README.md b/src/dcaiti_control/README.md new file mode 100644 index 0000000..d2f2f88 --- /dev/null +++ b/src/dcaiti_control/README.md @@ -0,0 +1,7 @@ +## Robot Package Template + +This is a GitHub template. You can make your own copy by clicking the green "Use this template" button. + +It is recommended that you keep the repo/package name the same, but if you do change it, ensure you do a "Find all" using your IDE (or the built-in GitHub IDE by hitting the `.` key) and rename all instances of `my_bot` to whatever your project's name is. + +Note that each directory currently has at least one file in it to ensure that git tracks the files (and, consequently, that a fresh clone has direcctories present for CMake to find). These example files can be removed if required (and the directories can be removed if `CMakeLists.txt` is adjusted accordingly). \ No newline at end of file diff --git a/src/dcaiti_control/config/dcaiti_config.yml b/src/dcaiti_control/config/dcaiti_config.yml new file mode 100644 index 0000000..910ef1c --- /dev/null +++ b/src/dcaiti_control/config/dcaiti_config.yml @@ -0,0 +1,25 @@ +controller_manager: + ros__parameters: + update_rate: 30 + + diff_cont: + type: diff_drive_controller/DiffDriveController + + joint_broad: + type: joint_state_broadcaster/JointStateBroadcaster + +diff_cont: + ros__parameters: + + publish_rate: 30.0 # You can set this higher than the controller manager update rate, but it will be throttled + base_frame_id: base_link + + left_wheel_names: ['left_wheel_joint'] + right_wheel_names: ['right_wheel_joint'] + wheel_separation: 0.215 + wheel_radius: 0.0477464829 + + use_stamped_vel: false + +# joint_broad: +# ros__parameters: \ No newline at end of file diff --git a/src/dcaiti_control/config/empty.yaml b/src/dcaiti_control/config/empty.yaml new file mode 100644 index 0000000..04700ba --- /dev/null +++ b/src/dcaiti_control/config/empty.yaml @@ -0,0 +1 @@ +# This is an empty file, so that git commits the folder correctly \ No newline at end of file diff --git a/src/dcaiti_control/config/gaz_ros2_ctl_sim.yml b/src/dcaiti_control/config/gaz_ros2_ctl_sim.yml new file mode 100644 index 0000000..48b48be --- /dev/null +++ b/src/dcaiti_control/config/gaz_ros2_ctl_sim.yml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + use_sim_time: true \ No newline at end of file diff --git a/src/dcaiti_control/config/gazebo_config.yml b/src/dcaiti_control/config/gazebo_config.yml new file mode 100644 index 0000000..d7caf6d --- /dev/null +++ b/src/dcaiti_control/config/gazebo_config.yml @@ -0,0 +1,3 @@ +gazebo: + ros__parameters: + publish_rate: 400.0 \ No newline at end of file diff --git a/src/dcaiti_control/description/camera.xacro b/src/dcaiti_control/description/camera.xacro new file mode 100644 index 0000000..a01d664 --- /dev/null +++ b/src/dcaiti_control/description/camera.xacro @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Red + + + 0 0 0 0 0 0 + true + 10 + + 1.089 + + R8G8B8 + 640 + 480 + + + 0.05 + 8.0 + + + + camera_link_optical + + + + + diff --git a/src/dcaiti_control/description/colours.xacro b/src/dcaiti_control/description/colours.xacro new file mode 100644 index 0000000..008bdb6 --- /dev/null +++ b/src/dcaiti_control/description/colours.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/gazebo_control.xacro b/src/dcaiti_control/description/gazebo_control.xacro new file mode 100644 index 0000000..45c10f2 --- /dev/null +++ b/src/dcaiti_control/description/gazebo_control.xacro @@ -0,0 +1,36 @@ + + + + + + + 30 + + left_wheel_joint + right_wheel_joint + 0.35 + 0.1 + + + + 200 + 10.0 + + + + cmd_vel + + + true + true + true + + odom + odom + base_link + + + + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/inertial_macros.xacro b/src/dcaiti_control/description/inertial_macros.xacro new file mode 100644 index 0000000..5cf1cac --- /dev/null +++ b/src/dcaiti_control/description/inertial_macros.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/lidar.xacro b/src/dcaiti_control/description/lidar.xacro new file mode 100644 index 0000000..1ef72bc --- /dev/null +++ b/src/dcaiti_control/description/lidar.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Red + + + 0 0 0 0 0 0 + true + 10 + + + + 360 + -3.14 + 3.14 + + + + 0.3 + 12 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/real_robot_control.xacro b/src/dcaiti_control/description/real_robot_control.xacro new file mode 100644 index 0000000..831da48 --- /dev/null +++ b/src/dcaiti_control/description/real_robot_control.xacro @@ -0,0 +1,37 @@ + + + + + diffdrive_arduino/DiffDriveArduino + left_wheel_joint + right_wheel_joint + 30 + /dev/ttyUSB0 + 115200 + 1000 + 3436 + + + + + -0.5 + 0.5 + + + + + + + -0.5 + 0.5 + + + + + + + + $(find dcaiti_control)/config/dcaiti_config.yml + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/robot.urdf.xacro b/src/dcaiti_control/description/robot.urdf.xacro new file mode 100644 index 0000000..f1e5c8a --- /dev/null +++ b/src/dcaiti_control/description/robot.urdf.xacro @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/robot_core.xacro b/src/dcaiti_control/description/robot_core.xacro new file mode 100644 index 0000000..fd4ed23 --- /dev/null +++ b/src/dcaiti_control/description/robot_core.xacro @@ -0,0 +1,166 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/White + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + \ No newline at end of file diff --git a/src/dcaiti_control/description/ros2_control.xacro b/src/dcaiti_control/description/ros2_control.xacro new file mode 100644 index 0000000..a3d9c22 --- /dev/null +++ b/src/dcaiti_control/description/ros2_control.xacro @@ -0,0 +1,30 @@ + + + + + gazebo_ros2_control/GazeboSystem + + + + -0.5 + 0.5 + + + + + + + -0.5 + 0.5 + + + + + + + + $(find dcaiti_control)/config/dcaiti_config.yml + $(find dcaiti_control)/config/gaz_ros2_ctl_sim.yml + + + \ No newline at end of file diff --git a/src/dcaiti_control/launch/launch_robot.py b/src/dcaiti_control/launch/launch_robot.py new file mode 100644 index 0000000..1f11f8e --- /dev/null +++ b/src/dcaiti_control/launch/launch_robot.py @@ -0,0 +1,91 @@ +import os + +from launch.substitutions import Command + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription + + +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessStart + + +from launch_ros.actions import Node +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit + + + +def generate_launch_description(): + + + # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled + # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!! + + package_name='dcaiti_control' #<--- CHANGE ME + + use_ros2_control = LaunchConfiguration('use_ros2_control') + + rsp = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory(package_name),'launch','rsp.launch.py' + )]), launch_arguments={'use_sim_time': 'false', 'use_ros2_control': 'true'}.items() + ) + + robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description']) + + controller_params = os.path.join( + get_package_share_directory('dcaiti_control'), # <-- Replace with your package name + 'config', + 'dcaiti_config.yml' + ) + + controller_manager = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[{'robot_description': robot_description}, + controller_params], + ) + delayed_controller_manager = TimerAction(period=3.0,actions=[controller_manager]) + + diff_drive_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diff_cont"], + ) + + joint_broad_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_broad"], + ) + + delayed_diff_drive_spawner = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=controller_manager, + on_start=[diff_drive_spawner], + ) + ) + delayed_joint_broad_spawner = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=controller_manager, + on_start=[joint_broad_spawner], + ) + ) + + + # Launch them all! + return LaunchDescription([ + DeclareLaunchArgument( + 'use_ros2_control', + default_value='true', + description='Use ros2_control if true'), + rsp, + delayed_controller_manager, + delayed_diff_drive_spawner, + delayed_joint_broad_spawner + ]) diff --git a/src/dcaiti_control/launch/launch_sim.py b/src/dcaiti_control/launch/launch_sim.py new file mode 100644 index 0000000..30bbc90 --- /dev/null +++ b/src/dcaiti_control/launch/launch_sim.py @@ -0,0 +1,89 @@ +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument + + +from launch_ros.actions import Node +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit + + + +def generate_launch_description(): + + + # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled + # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!! + + package_name='dcaiti_control' #<--- CHANGE ME + + use_ros2_control = LaunchConfiguration('use_ros2_control') + + rsp = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory(package_name),'launch','rsp.launch.py' + )]), launch_arguments={'use_sim_time': 'true', 'use_ros2_control': use_ros2_control}.items() + ) + gazebo_params_path = os.path.join(get_package_share_directory(package_name),'config','gazebo_config.yml') + # Include the Gazebo launch file, provided by the gazebo_ros package + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join( + get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]), + launch_arguments={'params_file': gazebo_params_path}.items() + ) + + # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot. + spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', + arguments=[ + '-topic', 'robot_description', + '-entity', 'dcaiti_control' + ], + output='screen' + ) + + diff_drive_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["diff_cont"], + ) + + joint_broad_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_broad"], + ) + + delayed_joint_broad_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[joint_broad_spawner], + ) + ) + + delayed_diff_drive_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity, + on_exit=[diff_drive_spawner], + ) + ) + + + # Launch them all! + return LaunchDescription([ + DeclareLaunchArgument( + 'use_ros2_control', + default_value='true', + description='Use ros2_control if true'), + rsp, + gazebo, + spawn_entity, + delayed_diff_drive_spawner, + delayed_joint_broad_spawner + ]) diff --git a/src/dcaiti_control/launch/rsp.launch.py b/src/dcaiti_control/launch/rsp.launch.py new file mode 100644 index 0000000..ffee96c --- /dev/null +++ b/src/dcaiti_control/launch/rsp.launch.py @@ -0,0 +1,54 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import Command + +import xacro + + +def generate_launch_description(): + + # Check if we're told to use sim time + use_sim_time = LaunchConfiguration('use_sim_time') + use_ros2_control = LaunchConfiguration('use_ros2_control') + + # Process the URDF file + pkg_path = os.path.join(get_package_share_directory('dcaiti_control')) + xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro') + robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' use_sim:=', use_sim_time]) + + # Create a robot_state_publisher node + params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time} + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + node_joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher' + ) + + + + # Launch! + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use sim time if true'), + DeclareLaunchArgument( + 'use_ros2_control', + default_value='false', + description='Use ros2_control if true'), + + node_robot_state_publisher, + node_joint_state_publisher + ]) diff --git a/src/dcaiti_control/package.xml b/src/dcaiti_control/package.xml new file mode 100644 index 0000000..cf0ecc2 --- /dev/null +++ b/src/dcaiti_control/package.xml @@ -0,0 +1,18 @@ + + + + dcaiti_control + 0.0.0 + TODO: Package description + MY NAME + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/dcaiti_control/worlds/empty.world b/src/dcaiti_control/worlds/empty.world new file mode 100644 index 0000000..5321672 --- /dev/null +++ b/src/dcaiti_control/worlds/empty.world @@ -0,0 +1,13 @@ + + + + + + model://sun + + + + model://ground_plane + + + diff --git a/src/diffdrive_arduino b/src/diffdrive_arduino deleted file mode 160000 index 25f4fbd..0000000 --- a/src/diffdrive_arduino +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 25f4fbd0db29319fc610debace6861ec48b75f81 diff --git a/src/diffdrive_arduino/.DS_Store b/src/diffdrive_arduino/.DS_Store new file mode 100644 index 0000000..40fee54 Binary files /dev/null and b/src/diffdrive_arduino/.DS_Store differ diff --git a/src/diffdrive_arduino/CMakeLists.txt b/src/diffdrive_arduino/CMakeLists.txt new file mode 100644 index 0000000..c0ea692 --- /dev/null +++ b/src/diffdrive_arduino/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.5) +project(diffdrive_arduino) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(serial REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(pluginlib REQUIRED) + + + + + +add_library(diffdrive_arduino SHARED src/diffdrive_arduino.cpp src/wheel.cpp src/arduino_comms.cpp) +target_include_directories( + diffdrive_arduino + PRIVATE + include +) +ament_target_dependencies( + diffdrive_arduino + hardware_interface + controller_manager + serial + rclcpp + rclcpp_lifecycle + pluginlib +) + +pluginlib_export_plugin_description_file(hardware_interface robot_hardware.xml) + + + +add_library(fake_robot SHARED src/fake_robot.cpp src/wheel.cpp) +set_property(TARGET fake_robot PROPERTY POSITION_INDEPENDENT_CODE ON) + +target_include_directories( + fake_robot + PRIVATE + include +) +ament_target_dependencies( + fake_robot + hardware_interface + controller_manager + rclcpp + rclcpp_lifecycle + pluginlib +) + +pluginlib_export_plugin_description_file(hardware_interface fake_robot_hardware.xml) + + + + +install( + TARGETS diffdrive_arduino fake_robot + DESTINATION lib +) + + + + +install( + DIRECTORY controllers launch/ + DESTINATION share/${PROJECT_NAME} +) + + +ament_export_libraries( + diffdrive_arduino + fake_robot +) +ament_export_dependencies( + hardware_interface + controller_manager + serial + rclcpp + pluginlib +) + +ament_package() + + diff --git a/src/diffdrive_arduino/LICENSE b/src/diffdrive_arduino/LICENSE new file mode 100644 index 0000000..e3636e5 --- /dev/null +++ b/src/diffdrive_arduino/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2020, Josh Newans +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. 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. + +3. Neither the name of the copyright holder 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. diff --git a/src/diffdrive_arduino/README.md b/src/diffdrive_arduino/README.md new file mode 100644 index 0000000..08b6701 --- /dev/null +++ b/src/diffdrive_arduino/README.md @@ -0,0 +1,5 @@ +# diffdrive_arduino + +**Adds ROS2 Galactic and Humble support** + +This node is designed to provide an interface between a `diff_drive_controller` from `ros_control` and an Arduino running firmware from `ros_arduino_bridge`. diff --git a/src/diffdrive_arduino/controllers/robot_controller_example.yaml b/src/diffdrive_arduino/controllers/robot_controller_example.yaml new file mode 100644 index 0000000..f69f505 --- /dev/null +++ b/src/diffdrive_arduino/controllers/robot_controller_example.yaml @@ -0,0 +1,43 @@ +controller_manager: + ros__parameters: + update_rate: 50 # Hz + + diff_controller: + type: diff_drive_controller/DiffDriveController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +diff_controller: + ros__parameters: + publish_rate: 50.0 + left_wheel_names: ['left_wheel_joint'] + right_wheel_names: ['right_wheel_joint'] + wheels_per_side: 1 + wheel_separation: 0.3 + wheel_radius: 0.05 + base_frame_id: base_link + use_stamped_vel: false + + linear.x.has_velocity_limits: false + linear.x.has_acceleration_limits: false + linear.x.has_jerk_limits: false + linear.x.max_velocity: 0.0 + linear.x.min_velocity: 0.0 + linear.x.max_acceleration: 0.0 + linear.x.min_acceleration: 0.0 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + + angular.z.has_velocity_limits: false + angular.z.has_acceleration_limits: false + angular.z.has_jerk_limits: false + angular.z.max_velocity: 0.0 + angular.z.min_velocity: 0.0 + angular.z.max_acceleration: 0.0 + angular.z.min_acceleration: 0.0 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 + +# joint_state_broadcaster: +# ros__parameters: diff --git a/src/diffdrive_arduino/fake_robot_hardware.xml b/src/diffdrive_arduino/fake_robot_hardware.xml new file mode 100644 index 0000000..e0ae050 --- /dev/null +++ b/src/diffdrive_arduino/fake_robot_hardware.xml @@ -0,0 +1,9 @@ + + + + The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. + + + diff --git a/src/diffdrive_arduino/include/.DS_Store b/src/diffdrive_arduino/include/.DS_Store new file mode 100644 index 0000000..aabfe3f Binary files /dev/null and b/src/diffdrive_arduino/include/.DS_Store differ diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/.DS_Store b/src/diffdrive_arduino/include/diffdrive_arduino/.DS_Store new file mode 100644 index 0000000..f7f9506 Binary files /dev/null and b/src/diffdrive_arduino/include/diffdrive_arduino/.DS_Store differ diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h b/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h new file mode 100644 index 0000000..21e4ff6 --- /dev/null +++ b/src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h @@ -0,0 +1,60 @@ +#ifndef DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H +#define DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H + +#include +#include +#include "rclcpp/rclcpp.hpp" + + +typedef enum frametypes { + VALUE = 0x00, REQUEST = 0x20, RESPONSE = 0x40 +} U_FrameType; + +typedef enum components { + DEFAULT_COMPONENT = 0x00, LEFT_MOTOR = 0x10, RIGHT_MOTOR = 0x20, BOTH_MOTORS = 0x30 +} U_Component; + +typedef enum requests { + DEFAULT_REQUEST, DRIVE_MOTOR, RESET_TIMESTAMP, DRIVE_ENCODER, PID_PARAMETER +} U_Request; + +class ArduinoComms +{ + + +public: + + ArduinoComms() + : logger_(rclcpp::get_logger("ArduinoCOMS")) + { } + + ArduinoComms(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms) + : serial_conn_(serial_device, baud_rate, serial::Timeout::simpleTimeout(timeout_ms)), logger_(rclcpp::get_logger("ArduinoCOMS")) + { } + + void setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms); + void sendEmptyMsg(); + void readEncoderValues(double &val_1, double &val_2); + void setMotorValues(float val_1, float val_2); + void setPidValues(float k_p, float k_d, float k_i, float k_o); + const std::vector buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector payload); + void readSpeed(double &left_speed, double &right_speed); + + bool connected() const { return serial_conn_.isOpen(); } + + void sendMsg(const std::vector &msg_to_send, bool print_output = false); + + +private: + serial::Serial serial_conn_; ///< Underlying serial connection + std::vector marshalling(float number); + std::vector marshalling(uint16_t number); + std::vector marshalling(uint32_t number); + void safeRead(std::vector &buffer, int bytes_to_read); + rclcpp::Logger logger_; + + + +}; + +#endif // DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/config.h b/src/diffdrive_arduino/include/diffdrive_arduino/config.h new file mode 100644 index 0000000..04d682d --- /dev/null +++ b/src/diffdrive_arduino/include/diffdrive_arduino/config.h @@ -0,0 +1,19 @@ +#ifndef DIFFDRIVE_ARDUINO_CONFIG_H +#define DIFFDRIVE_ARDUINO_CONFIG_H + +#include + + +struct Config +{ + std::string left_wheel_name = "left_wheel"; + std::string right_wheel_name = "right_wheel"; + float loop_rate = 30; + std::string device = "/dev/ttyUSB0"; + int baud_rate = 115200; + int timeout = 1000; + int enc_counts_per_rev = 1920; +}; + + +#endif // DIFFDRIVE_ARDUINO_CONFIG_H \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h b/src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h new file mode 100644 index 0000000..e3d22cc --- /dev/null +++ b/src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h @@ -0,0 +1,61 @@ +#ifndef DIFFDRIVE_ARDUINO_REAL_ROBOT_H +#define DIFFDRIVE_ARDUINO_REAL_ROBOT_H + +#include +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + +#include "config.h" +#include "wheel.h" +#include "arduino_comms.h" + +using hardware_interface::CallbackReturn; +using hardware_interface::return_type; + +namespace diffdrive_arduino +{ + +class DiffDriveArduino : public hardware_interface::SystemInterface +{ + + +public: + DiffDriveArduino(); + + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + + +private: + + Config cfg_; + ArduinoComms arduino_; + + Wheel l_wheel_; + Wheel r_wheel_; + + rclcpp::Logger logger_; + + std::chrono::time_point time_; + +}; +} // namespace diffdrive_arduino + +#endif // DIFFDRIVE_ARDUINO_REAL_ROBOT_H \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h b/src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h new file mode 100644 index 0000000..6416a75 --- /dev/null +++ b/src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h @@ -0,0 +1,55 @@ +#ifndef DIFFDRIVE_ARDUINO_FAKE_ROBOT_H +#define DIFFDRIVE_ARDUINO_FAKE_ROBOT_H + + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + +#include "config.h" +#include "wheel.h" + +using hardware_interface::CallbackReturn; +using hardware_interface::return_type; + +class FakeRobot : public hardware_interface::SystemInterface +{ + + +public: + FakeRobot(); + + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + +private: + + Config cfg_; + + Wheel l_wheel_; + Wheel r_wheel_; + + rclcpp::Logger logger_; + + std::chrono::time_point time_; + +}; + + +#endif // DIFFDRIVE_ARDUINO_FAKE_ROBOT_H \ No newline at end of file diff --git a/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h b/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h new file mode 100644 index 0000000..712f25e --- /dev/null +++ b/src/diffdrive_arduino/include/diffdrive_arduino/wheel.h @@ -0,0 +1,34 @@ +#ifndef DIFFDRIVE_ARDUINO_WHEEL_H +#define DIFFDRIVE_ARDUINO_WHEEL_H + +#include + + + +class Wheel +{ + public: + + std::string name = ""; + int enc = 0; + double cmd = 0; + double pos = 0; + double vel = 0; + double eff = 0; + double velSetPt = 0; + double rads_per_count = 0; + + Wheel() = default; + + Wheel(const std::string &wheel_name, int counts_per_rev); + + void setup(const std::string &wheel_name, int counts_per_rev); + + double calcEncAngle(); + + + +}; + + +#endif // DIFFDRIVE_ARDUINO_WHEEL_H \ No newline at end of file diff --git a/src/diffdrive_arduino/launch/__pycache__/test_robot.launch.cpython-310.pyc b/src/diffdrive_arduino/launch/__pycache__/test_robot.launch.cpython-310.pyc new file mode 100644 index 0000000..5b76a06 Binary files /dev/null and b/src/diffdrive_arduino/launch/__pycache__/test_robot.launch.cpython-310.pyc differ diff --git a/src/diffdrive_arduino/launch/fake_robot.launch.py b/src/diffdrive_arduino/launch/fake_robot.launch.py new file mode 100644 index 0000000..d848df0 --- /dev/null +++ b/src/diffdrive_arduino/launch/fake_robot.launch.py @@ -0,0 +1,52 @@ +# Copyright 2020 ROS2-Control Development Team (2020) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_path = os.path.join( + get_package_share_directory('diffdrive_arduino'), + 'description', + 'robot.urdf') + robot_description_config = xacro.process_file(robot_description_path) + robot_description = {'robot_description': robot_description_config.toxml()} + + test_controller = os.path.join( + get_package_share_directory('diffdrive_arduino'), + 'controllers', + 'fake_robot_controller.yaml' + ) + + return LaunchDescription([ + Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, test_controller], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + ) + + ]) diff --git a/src/diffdrive_arduino/launch/test_robot.launch.py b/src/diffdrive_arduino/launch/test_robot.launch.py new file mode 100644 index 0000000..d1bc601 --- /dev/null +++ b/src/diffdrive_arduino/launch/test_robot.launch.py @@ -0,0 +1,52 @@ +# Copyright 2020 ROS2-Control Development Team (2020) +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + + # Get URDF via xacro + robot_description_path = os.path.join( + get_package_share_directory('dcaiti_control'), + 'description', + 'robot.urdf.xacro') + robot_description_config = xacro.process_file(robot_description_path) + robot_description = {'robot_description': robot_description_config.toxml()} + + test_controller = os.path.join( + get_package_share_directory('diffdrive_arduino'), + 'controllers', + 'robot_controller.yaml' + ) + + return LaunchDescription([ + Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, test_controller], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + ) + + ]) diff --git a/src/diffdrive_arduino/package.xml b/src/diffdrive_arduino/package.xml new file mode 100644 index 0000000..cf67b66 --- /dev/null +++ b/src/diffdrive_arduino/package.xml @@ -0,0 +1,37 @@ + + + diffdrive_arduino + 0.0.1 + Provides an interface between diff_drive_controller and an Arduino. + + Josh Newans + Josh Newans + BSD + + + + https://github.com/joshnewans/diffdrive_arduino/issues + https://github.com/joshnewans/diffdrive_arduino + + + + ament_cmake + + hardware_interface + controller_manager + pluginlib + rclcpp + serial + rclcpp_lifecycle + + + +ament_add_gmock +hardware_interface + + + ament_cmake + + + + diff --git a/src/diffdrive_arduino/robot_hardware.xml b/src/diffdrive_arduino/robot_hardware.xml new file mode 100644 index 0000000..5687dfd --- /dev/null +++ b/src/diffdrive_arduino/robot_hardware.xml @@ -0,0 +1,9 @@ + + + + Hardware Interface for Differential Drive Arduino controller. + + + diff --git a/src/diffdrive_arduino/src/.DS_Store b/src/diffdrive_arduino/src/.DS_Store new file mode 100644 index 0000000..0f8a2d8 Binary files /dev/null and b/src/diffdrive_arduino/src/.DS_Store differ diff --git a/src/diffdrive_arduino/src/arduino_comms.cpp b/src/diffdrive_arduino/src/arduino_comms.cpp new file mode 100644 index 0000000..5d03e04 --- /dev/null +++ b/src/diffdrive_arduino/src/arduino_comms.cpp @@ -0,0 +1,169 @@ +#include "diffdrive_arduino/arduino_comms.h" +// #include +#include +#include +#include +#include +#include + + +void ArduinoComms::setup(const std::string &serial_device, int32_t baud_rate, int32_t timeout_ms) +{ + serial_conn_.setPort(serial_device); + serial_conn_.setBaudrate(baud_rate); + serial::Timeout tt = serial::Timeout::simpleTimeout(timeout_ms); + serial_conn_.setTimeout(tt); // This should be inline except setTimeout takes a reference and so needs a variable + serial_conn_.open(); + +} + +void ArduinoComms::sendEmptyMsg() +{ +} + +void ArduinoComms::readEncoderValues(double &left_speed, double &right_speed) +{ + std::vector payload = {DRIVE_ENCODER}; + std::vector msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload); + sendMsg(msg); + readSpeed(left_speed, right_speed); +} + +void ArduinoComms::setMotorValues(float left_speed, float right_speed) +{ + std::vector payload = {DRIVE_MOTOR}; + auto bytes_left_speed = marshalling(left_speed); + payload.insert(payload.end(), bytes_left_speed.begin(), bytes_left_speed.end()); + auto bytes_right_speed = marshalling(right_speed); + payload.insert(payload.end(), bytes_right_speed.begin(), bytes_right_speed.end()); + + std::vector msg = buildMsg(REQUEST, BOTH_MOTORS, 0, payload); + + sendMsg(msg); +} + +void ArduinoComms::setPidValues(float k_p, float k_d, float k_i, float k_o) +{ + std::stringstream ss; + ss << "u " << k_p << ":" << k_d << ":" << k_i << ":" << k_o << "\r"; +} + +void ArduinoComms::readSpeed(double &left_speed, double &right_speed) +{ + std::vector buffer; + int read_bytes = 0; + do{ + buffer.clear(); + read_bytes = serial_conn_.read(buffer, 1); + } + while(read_bytes == 0 || buffer.at(0) != 0xAA); + + // read frame and extract two big-endian floats from payload + safeRead(buffer, 1); + uint8_t frame_type = buffer.at(0); + safeRead(buffer, 1); + uint8_t payload_size = buffer.at(0); + safeRead(buffer, 1); + uint8_t frame_component = buffer.at(0); + safeRead(buffer, 4); + uint32_t time = *reinterpret_cast(&buffer[0]); + + safeRead(buffer, payload_size); + std::vector payload(buffer.begin(), buffer.end()); + // read checksum + safeRead(buffer, 2); + uint16_t checksum = buffer.at(0); + checksum = checksum << 8; + checksum = checksum | buffer.at(1); + + // check checksum of payload + uint16_t payload_checksum = 0; + for (auto it = payload.begin(); it != payload.end(); ++it) + { + payload_checksum += *it; + } + if (payload_checksum != checksum) + { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Checksum error"); + } + + if (frame_type == VALUE && frame_component == BOTH_MOTORS) + { + std::vector left_speed_bytes(payload.begin(), payload.begin() + 4); + std::vector right_speed_bytes(payload.begin() + 4, payload.end()); + std::reverse(left_speed_bytes.begin(), left_speed_bytes.end()); + std::reverse(right_speed_bytes.begin(), right_speed_bytes.end()); + float left_speed_float = *reinterpret_cast(&left_speed_bytes[0]); + float right_speed_float = *reinterpret_cast(&right_speed_bytes[0]); + left_speed = left_speed_float; + right_speed = right_speed_float; + } +} + +void ArduinoComms::safeRead(std::vector &buffer, int bytes_to_read){ + buffer.clear(); + int read_bytes = 0; + do{ + read_bytes += serial_conn_.read(buffer, bytes_to_read - read_bytes); + } + while(read_bytes < bytes_to_read); +} + +std::vector ArduinoComms::marshalling(uint16_t number) +{ + number = htons(number); + uint8_t array[2]; + array[0]=number & 0xff; + array[1]=(number >> 8); + std::vector vec(array, array + sizeof array / sizeof array[0]); + return vec; +} + +std::vector ArduinoComms::marshalling(uint32_t number) +{ + number = htons(number); + uint8_t array[4]; + array[0]=number & 0xff; + array[1]=(number >> 8) & 0xff; + array[2]=(number >> 16) & 0xff; + array[3]=(number >> 24) & 0xff; + std::vector vec(array, array + sizeof array / sizeof array[0]); + return vec; +} + +std::vector ArduinoComms::marshalling(float number) +{ + uint32_t int_interpretation = reinterpret_cast( number ); + union { + float f; + uint8_t b[4]; + } u; + u.f = number; + std::vector vec(u.b, u.b + sizeof u.b / sizeof u.b[0]); + std::reverse(vec.begin(), vec.end()); + return vec; +} + +const std::vector ArduinoComms::buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector payload) +{ + std::vector buffer; + buffer.push_back(0xAA); + buffer.push_back(frame_type); + buffer.push_back(payload.size()); + buffer.push_back(frame_component); + uint32_t time_zero = 0; + auto byte_time = marshalling(time_zero); + buffer.insert(buffer.end(), byte_time.begin(), byte_time.end()); + uint16_t checksum = std::accumulate(payload.begin(), payload.end(), 0); + buffer.insert(buffer.end(), payload.begin(), payload.end()); + auto byte_checksum = marshalling(checksum); + buffer.insert(buffer.end(), byte_checksum.begin(), byte_checksum.end()); + + return buffer; +} + + +void ArduinoComms::sendMsg(const std::vector &msg_to_send, bool print_output) +{ + serial_conn_.write(msg_to_send); +} \ No newline at end of file diff --git a/src/diffdrive_arduino/src/diffdrive_arduino.cpp b/src/diffdrive_arduino/src/diffdrive_arduino.cpp new file mode 100644 index 0000000..8551774 --- /dev/null +++ b/src/diffdrive_arduino/src/diffdrive_arduino.cpp @@ -0,0 +1,149 @@ +#include "diffdrive_arduino/diffdrive_arduino.h" + + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace diffdrive_arduino +{ + + +DiffDriveArduino::DiffDriveArduino() + : logger_(rclcpp::get_logger("DiffDriveArduino")) +{} + +CallbackReturn DiffDriveArduino::on_init(const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) +{ + return CallbackReturn::ERROR; +} + + RCLCPP_INFO(logger_, "Configuring..."); + + time_ = std::chrono::system_clock::now(); + + cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"]; + cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"]; + cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]); + cfg_.device = info_.hardware_parameters["device"]; + cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]); + cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]); + cfg_.enc_counts_per_rev = std::stoi(info_.hardware_parameters["enc_counts_per_rev"]); + + // Set up the wheels + l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); + r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); + + // Set up the Arduino + arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); + + RCLCPP_INFO(logger_, "Finished Configuration"); + + return CallbackReturn::SUCCESS; +} + +std::vector DiffDriveArduino::export_state_interfaces() +{ + // We need to set up a position and a velocity interface for each wheel + + std::vector state_interfaces; + + state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel)); + state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos)); + state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel)); + state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos)); + + return state_interfaces; +} + +std::vector DiffDriveArduino::export_command_interfaces() +{ + // We need to set up a velocity command interface for each wheel + + std::vector command_interfaces; + + command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd)); + + return command_interfaces; +} + + +CallbackReturn DiffDriveArduino::on_activate(const rclcpp_lifecycle::State & /* previous_state */) +{ + RCLCPP_INFO(logger_, "Starting Controller..."); + arduino_.sendEmptyMsg(); + // arduino.setPidValues(9,7,0,100); + // arduino.setPidValues(14,7,0,100); + arduino_.setPidValues(30, 20, 0, 100); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn DiffDriveArduino::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) +{ + RCLCPP_INFO(logger_, "Stopping Controller..."); + + return CallbackReturn::SUCCESS; +} + +return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + + // TODO fix chrono duration + + // Calculate time delta + auto new_time = std::chrono::system_clock::now(); + std::chrono::duration diff = new_time - time_; + double deltaSeconds = diff.count(); + time_ = new_time; + + + if (!arduino_.connected()) + { + return return_type::ERROR; + } + + arduino_.readEncoderValues(l_wheel_.vel , r_wheel_.vel); + + // Force the wheel position + l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; + r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; + + + + return return_type::OK; + + +} + +return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + + if (!arduino_.connected()) + { + return return_type::ERROR; + } + + arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd); + + + + + return return_type::OK; + + + +} + +} // namespace diffdrive_arduino + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + diffdrive_arduino::DiffDriveArduino, + hardware_interface::SystemInterface +) diff --git a/src/diffdrive_arduino/src/diffdrive_robot.cpp b/src/diffdrive_arduino/src/diffdrive_robot.cpp new file mode 100644 index 0000000..34dd02f --- /dev/null +++ b/src/diffdrive_arduino/src/diffdrive_robot.cpp @@ -0,0 +1,39 @@ +#include +#include +#include "diffdrive_arduino/diffdrive_arduino.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "diffdrive_robot"); + ros::NodeHandle n("~"); + + DiffDriveArduino::Config robot_cfg; + + // Attempt to retrieve parameters. If they don't exist, the default values from the struct will be used + n.getParam("left_wheel_name", robot_cfg.left_wheel_name); + n.getParam("right_wheel_name", robot_cfg.right_wheel_name); + n.getParam("baud_rate", robot_cfg.baud_rate); + n.getParam("device", robot_cfg.device); + n.getParam("enc_counts_per_rev", robot_cfg.enc_counts_per_rev); + n.getParam("robot_loop_rate", robot_cfg.loop_rate); + + + DiffDriveArduino robot(robot_cfg); + controller_manager::ControllerManager cm(&robot); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + ros::Time prevTime = ros::Time(0); //ros::Time::now(); + + ros::Rate loop_rate(10); + + while (ros::ok()) + { + robot.read(); + cm.update(robot.get_time(), robot.get_period()); + robot.write(); + + loop_rate.sleep(); + } +} diff --git a/src/diffdrive_arduino/src/fake_robot.cpp b/src/diffdrive_arduino/src/fake_robot.cpp new file mode 100644 index 0000000..6794043 --- /dev/null +++ b/src/diffdrive_arduino/src/fake_robot.cpp @@ -0,0 +1,117 @@ +#include "diffdrive_arduino/fake_robot.h" + + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "rclcpp/rclcpp.hpp" +#include + + +CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) +{ + return CallbackReturn::ERROR; +} + + RCLCPP_INFO(logger_, "Configuring..."); + + time_ = std::chrono::system_clock::now(); + + cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"]; + cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"]; + + + // Set up the wheels + // Note: It doesn't matter that we haven't set encoder counts per rev + // since the fake robot bypasses the encoder code completely + + l_wheel_.setup(cfg_.left_wheel_name, cfg_.enc_counts_per_rev); + r_wheel_.setup(cfg_.right_wheel_name, cfg_.enc_counts_per_rev); + + RCLCPP_INFO(logger_, "Finished Configuration"); + + return CallbackReturn::SUCCESS; +} + +std::vector FakeRobot::export_state_interfaces() +{ + // We need to set up a position and a velocity interface for each wheel + + std::vector state_interfaces; + + state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel)); + state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos)); + state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel)); + state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos)); + + return state_interfaces; +} + +std::vector FakeRobot::export_command_interfaces() +{ + // We need to set up a velocity command interface for each wheel + + std::vector command_interfaces; + + command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd)); + + return command_interfaces; +} + + + +hardware_interface::return_type FakeRobot::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + + // TODO fix chrono duration + + // Calculate time delta + auto new_time = std::chrono::system_clock::now(); + std::chrono::duration diff = new_time - time_; + double deltaSeconds = diff.count(); + time_ = new_time; + + + // Force the wheel position + l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds; + r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds; + + return return_type::OK; + + +} + +hardware_interface::return_type FakeRobot::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + + // Set the wheel velocities to directly match what is commanded + + l_wheel_.vel = l_wheel_.cmd; + r_wheel_.vel = r_wheel_.cmd; + + + return return_type::OK; +} + +CallbackReturn FakeRobot::on_activate(const rclcpp_lifecycle::State & /* previous_state */) +{ + RCLCPP_INFO(logger_, "Starting Controller..."); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn FakeRobot::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */) +{ + RCLCPP_INFO(logger_, "Stopping Controller..."); + + return CallbackReturn::SUCCESS; +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + FakeRobot, + hardware_interface::SystemInterface +) \ No newline at end of file diff --git a/src/diffdrive_arduino/src/wheel.cpp b/src/diffdrive_arduino/src/wheel.cpp new file mode 100644 index 0000000..60d8ede --- /dev/null +++ b/src/diffdrive_arduino/src/wheel.cpp @@ -0,0 +1,21 @@ +#include "diffdrive_arduino/wheel.h" + +#include + + +Wheel::Wheel(const std::string &wheel_name, int counts_per_rev) +{ + setup(wheel_name, counts_per_rev); +} + + +void Wheel::setup(const std::string &wheel_name, int counts_per_rev) +{ + name = wheel_name; + rads_per_count = (2*M_PI)/counts_per_rev; +} + +double Wheel::calcEncAngle() +{ + return enc * rads_per_count; +} \ No newline at end of file