t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Removed accidental subrepos

Browse Source
This commit is contained in:
Tim Korjakow 2023-06-15 12:18:25 +02:00
parent 3604da839f
commit 5d0513a073
47 changed files with 2085 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/dcaiti_control

@ -1 +0,0 @@
Subproject commit d97823342390b8de78b2710dff6c557596e718bf

40
src/dcaiti_control/CMakeLists.txt Normal file
View File

@ -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(<dependency> 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()

201
src/dcaiti_control/LICENSE.md Normal file
View File

@ -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.

7
src/dcaiti_control/README.md Normal file
View File

@ -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).

25
src/dcaiti_control/config/dcaiti_config.yml Normal file
View File

@ -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:

1
src/dcaiti_control/config/empty.yaml Normal file
View File

@ -0,0 +1 @@
# This is an empty file, so that git commits the folder correctly

3
src/dcaiti_control/config/gaz_ros2_ctl_sim.yml Normal file
View File

@ -0,0 +1,3 @@
controller_manager:
ros__parameters:
use_sim_time: true

3
src/dcaiti_control/config/gazebo_config.yml Normal file
View File

@ -0,0 +1,3 @@
gazebo:
ros__parameters:
publish_rate: 400.0

55
src/dcaiti_control/description/camera.xacro Normal file
View File

@ -0,0 +1,55 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<joint name="camera_joint" type="fixed">
<parent link="chassis"/>
<child link="camera_link"/>
<origin xyz="0.305 0 0.08" rpy="0 0 0"/>
</joint>
<link name="camera_link">
<visual>
<geometry>
<box size="0.010 0.03 0.03"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="camera_optical_joint" type="fixed">
<parent link="camera_link"/>
<child link="camera_link_optical"/>
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
</joint>
<link name="camera_link_optical"></link>
<gazebo reference="camera_link">
<material>Gazebo/Red</material>
<sensor name="camera" type="camera">
<pose> 0 0 0 0 0 0 </pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<camera>
<horizontal_fov>1.089</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<frame_name>camera_link_optical</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>

22
src/dcaiti_control/description/colours.xacro Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<material name="orange">
<color rgba="1 0.3 0.1 1"/>
</material>
<material name="blue">
<color rgba="0.2 0.2 1 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</robot>

36
src/dcaiti_control/description/gazebo_control.xacro Normal file
View File

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<update_rate>30</update_rate>
<!-- Wheel Information -->
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.35</wheel_separation>
<wheel_diameter>0.1</wheel_diameter>
<!-- Limits -->
<max_wheel_torque>200</max_wheel_torque>
<max_wheel_acceleration>10.0</max_wheel_acceleration>
<!-- Output -->
<command_topic>cmd_vel</command_topic>
<!-- output -->
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
<odometry_topic>odom</odometry_topic>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
</plugin>
</gazebo>
</robot>

40
src/dcaiti_control/description/inertial_macros.xacro Normal file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
<!-- These make use of xacro's mathematical functionality -->
<xacro:macro name="inertial_sphere" params="mass radius *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}" />
<inertia ixx="${(2/5) * mass * (radius*radius)}" ixy="0.0" ixz="0.0"
iyy="${(2/5) * mass * (radius*radius)}" iyz="0.0"
izz="${(2/5) * mass * (radius*radius)}" />
</inertial>
</xacro:macro>
<xacro:macro name="inertial_box" params="mass x y z *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}" />
<inertia ixx="${(1/12) * mass * (y*y+z*z)}" ixy="0.0" ixz="0.0"
iyy="${(1/12) * mass * (x*x+z*z)}" iyz="0.0"
izz="${(1/12) * mass * (x*x+y*y)}" />
</inertial>
</xacro:macro>
<xacro:macro name="inertial_cylinder" params="mass length radius *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}" />
<inertia ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
izz="${(1/2) * mass * (radius*radius)}" />
</inertial>
</xacro:macro>
</robot>

61
src/dcaiti_control/description/lidar.xacro Normal file
View File

@ -0,0 +1,61 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<xacro:include filename="inertial_macros.xacro"/>
<joint name="laser_joint" type="fixed">
<parent link="chassis"/>
<child link="laser_frame"/>
<origin xyz="0.1 0 0.175" rpy="0 0 0"/>
</joint>
<link name="laser_frame">
<visual>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="laser_frame">
<material>Gazebo/Red</material>
<sensor name="laser" type="ray">
<pose> 0 0 0 0 0 0 </pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<min_angle>-3.14</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
</scan>
<range>
<min>0.3</min>
<max>12</max>
</range>
</ray>
<plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
<ros>
<argument>~/out:=scan</argument>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_frame</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>

37
src/dcaiti_control/description/real_robot_control.xacro Normal file
View File

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<ros2_control name="RealRobot" type="system">
<hardware>
<plugin>diffdrive_arduino/DiffDriveArduino</plugin>
<param name="left_wheel_name">left_wheel_joint</param>
<param name="right_wheel_name">right_wheel_joint</param>
<param name="loop_rate">30</param>
<param name="device">/dev/ttyUSB0</param>
<param name="baud_rate">115200</param>
<param name="timeout">1000</param>
<param name="enc_counts_per_rev">3436</param>
</hardware>
<!-- Note everything below here is the same as the Gazebo one -->
<joint name="left_wheel_joint">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
<joint name="right_wheel_joint">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
</ros2_control>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters>
</plugin>
</gazebo>
</robot>

31
src/dcaiti_control/description/robot.urdf.xacro Normal file
View File

@ -0,0 +1,31 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="dcaiti_control">
<!-- Define parameters-->
<xacro:arg name="use_ros2_control" default="true"/>
<xacro:arg name="use_sim" default="false"/>
<!-- Include the colours -->
<xacro:include filename="colours.xacro" />
<!-- Include the inertial macros -->
<xacro:include filename="inertial_macros.xacro"/>
<!-- Include the gazebo control -->
<xacro:if value="$(arg use_ros2_control)">
<xacro:if value="$(arg use_sim)">
<xacro:include filename="ros2_control.xacro" />
</xacro:if>
<xacro:unless value="$(arg use_sim)">
<xacro:include filename="real_robot_control.xacro" />
</xacro:unless>
</xacro:if>
<xacro:unless value="$(arg use_ros2_control)">
<xacro:include filename="gazebo_control.xacro" />
</xacro:unless>
<!-- Sensors -->
<xacro:include filename="camera.xacro" />
<xacro:include filename="lidar.xacro" />
<xacro:include filename="robot_core.xacro" />
</robot>

166
src/dcaiti_control/description/robot_core.xacro Normal file
View File

@ -0,0 +1,166 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="base_link">
</link>
<!-- CHASSIS LINK -->
<joint name="chassis_joint" type="fixed">
<parent link="base_link"/>
<child link="chassis"/>
<origin xyz="0 0 0"/>
</joint>
<link name="chassis">
<visual>
<origin xyz="0 0 0.035"/>
<geometry>
<box size="0.265 0.175 0.07"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.035"/>
<geometry>
<box size="0.265 0.175 0.07"/>
</geometry>
</collision>
<xacro:inertial_box mass="3.3" x="0.265" y="0.175" z="0.07">
<origin xyz="0 0 0.035" rpy="0 0 0"/>
</xacro:inertial_box>
</link>
<gazebo reference="chassis">
<material>Gazebo/White</material>
</gazebo>
<!-- LEFT WHEEL LINK -->
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="${0.3/(2*pi)}" length="0.04"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<!-- Better collosion behaviour than a cylinder -->
<sphere radius="${0.3/(2*pi)}"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" radius="${0.3/(2*pi)}" length="0.04">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 0.1075 0" rpy="-${pi/2} 0 0" />
<axis xyz="0 0 1"/>
</joint>
<gazebo reference="left_wheel">
<material>Gazebo/Grey</material>
</gazebo>
<!-- RIGHT WHEEL LINK -->
<link name="right_wheel">
<visual>
<geometry>
<!-- Better collosion behaviour than a cylinder -->
<cylinder radius="${0.3/(2*pi)}" length="0.04"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<sphere radius="${0.3/(2*pi)}" />
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" radius="${0.3/(2*pi)}" length="0.04">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0 -0.1075 0" rpy="${pi/2} 0 0" />
<axis xyz="0 0 -1"/>
</joint>
<gazebo reference="right_wheel">
<material>Gazebo/Grey</material>
</gazebo>
<!-- CASTER WHEEL LINK Front -->
<joint name="caster_wheel_joint_front" type="fixed">
<parent link="chassis"/>
<child link="caster_wheel_front"/>
<origin xyz="0.1 0 0"/>
</joint>
<link name="caster_wheel_front">
<visual>
<geometry>
<sphere radius="${0.3/(2*pi)}"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<sphere radius="${0.3/(2*pi)}"/>
</geometry>
</collision>
<xacro:inertial_sphere mass="0.1" radius="${0.3/(2*pi)}">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere>
</link>
<gazebo reference="caster_wheel_front">
<material>Gazebo/Black</material>
<mu1 value="0.001"/>
<mu2 value="0.001"/>
</gazebo>
<!-- CASTER WHEEL LINK BACK-->
<joint name="caster_wheel_joint_back" type="fixed">
<parent link="chassis"/>
<child link="caster_wheel_back"/>
<origin xyz="-0.1 0 0"/>
</joint>
<link name="caster_wheel_back">
<visual>
<geometry>
<sphere radius="${0.3/(2*pi)}"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<sphere radius="${0.3/(2*pi)}"/>
</geometry>
</collision>
<xacro:inertial_sphere mass="0.1" radius="${0.3/(2*pi)}">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere>
</link>
<gazebo reference="caster_wheel_back">
<material>Gazebo/Black</material>
<mu1 value="0.001"/>
<mu2 value="0.001"/>
</gazebo>
</robot>

30
src/dcaiti_control/description/ros2_control.xacro Normal file
View File

@ -0,0 +1,30 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="left_wheel_joint">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="position" />
<state_interface name="velocity" />
</joint>
<joint name="right_wheel_joint">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity" />
<state_interface name="position" />
</joint>
</ros2_control>
<gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
<parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters>
<parameters>$(find dcaiti_control)/config/gaz_ros2_ctl_sim.yml</parameters>
</plugin>
</gazebo>
</robot>

91
src/dcaiti_control/launch/launch_robot.py Normal file
View File

@ -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
])

89
src/dcaiti_control/launch/launch_sim.py Normal file
View File

@ -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
])

54
src/dcaiti_control/launch/rsp.launch.py Normal file
View File

@ -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
])

18
src/dcaiti_control/package.xml Normal file
View File

@ -0,0 +1,18 @@
<?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>dcaiti_control </name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="my_email@email.com">MY NAME</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

13
src/dcaiti_control/worlds/empty.world Normal file
View File

@ -0,0 +1,13 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
</world>
</sdf>

1
src/diffdrive_arduino

@ -1 +0,0 @@
Subproject commit 25f4fbd0db29319fc610debace6861ec48b75f81

BIN
src/diffdrive_arduino/.DS_Store vendored Normal file
View File

Binary file not shown.

102
src/diffdrive_arduino/CMakeLists.txt Normal file
View File

@ -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()

29
src/diffdrive_arduino/LICENSE Normal file
View File

@ -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.

5
src/diffdrive_arduino/README.md Normal file
View File

@ -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`.

43
src/diffdrive_arduino/controllers/robot_controller_example.yaml Normal file
View File

@ -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:

9
src/diffdrive_arduino/fake_robot_hardware.xml Normal file
View File

@ -0,0 +1,9 @@
<library path="fake_robot">
<class name="fake_robot/FakeRobot"
type="FakeRobot"
base_class_type="hardware_interface::SystemInterface">
<description>
The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
</description>
</class>
</library>

BIN
src/diffdrive_arduino/include/.DS_Store vendored Normal file
View File

Binary file not shown.

BIN
src/diffdrive_arduino/include/diffdrive_arduino/.DS_Store vendored Normal file
View File

Binary file not shown.

60
src/diffdrive_arduino/include/diffdrive_arduino/arduino_comms.h Normal file
View File

@ -0,0 +1,60 @@
#ifndef DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H
#define DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H
#include <serial/serial.h>
#include <cstring>
#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<uint8_t> buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector<uint8_t> payload);
void readSpeed(double &left_speed, double &right_speed);
bool connected() const { return serial_conn_.isOpen(); }
void sendMsg(const std::vector<uint8_t> &msg_to_send, bool print_output = false);
private:
serial::Serial serial_conn_; ///< Underlying serial connection
std::vector<uint8_t> marshalling(float number);
std::vector<uint8_t> marshalling(uint16_t number);
std::vector<uint8_t> marshalling(uint32_t number);
void safeRead(std::vector<uint8_t> &buffer, int bytes_to_read);
rclcpp::Logger logger_;
};
#endif // DIFFDRIVE_ARDUINO_ARDUINO_COMMS_H

19
src/diffdrive_arduino/include/diffdrive_arduino/config.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef DIFFDRIVE_ARDUINO_CONFIG_H
#define DIFFDRIVE_ARDUINO_CONFIG_H
#include <string>
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

61
src/diffdrive_arduino/include/diffdrive_arduino/diffdrive_arduino.h Normal file
View File

@ -0,0 +1,61 @@
#ifndef DIFFDRIVE_ARDUINO_REAL_ROBOT_H
#define DIFFDRIVE_ARDUINO_REAL_ROBOT_H
#include <cstring>
#include "rclcpp/rclcpp.hpp"
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#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<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> 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<std::chrono::system_clock> time_;
};
} // namespace diffdrive_arduino
#endif // DIFFDRIVE_ARDUINO_REAL_ROBOT_H

55
src/diffdrive_arduino/include/diffdrive_arduino/fake_robot.h Normal file
View File

@ -0,0 +1,55 @@
#ifndef DIFFDRIVE_ARDUINO_FAKE_ROBOT_H
#define DIFFDRIVE_ARDUINO_FAKE_ROBOT_H
#include "rclcpp/rclcpp.hpp"
#include <hardware_interface/handle.hpp>
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <rclcpp_lifecycle/state.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#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<hardware_interface::StateInterface> export_state_interfaces() override;
std::vector<hardware_interface::CommandInterface> 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<std::chrono::system_clock> time_;
};
#endif // DIFFDRIVE_ARDUINO_FAKE_ROBOT_H

34
src/diffdrive_arduino/include/diffdrive_arduino/wheel.h Normal file
View File

@ -0,0 +1,34 @@
#ifndef DIFFDRIVE_ARDUINO_WHEEL_H
#define DIFFDRIVE_ARDUINO_WHEEL_H
#include <string>
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

BIN
src/diffdrive_arduino/launch/__pycache__/test_robot.launch.cpython-310.pyc Normal file
View File

Binary file not shown.

52
src/diffdrive_arduino/launch/fake_robot.launch.py Normal file
View File

@ -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',
},
)
])

52
src/diffdrive_arduino/launch/test_robot.launch.py Normal file
View File

@ -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',
},
)
])

37
src/diffdrive_arduino/package.xml Normal file
View File

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<package format="2">
<name>diffdrive_arduino</name>
<version>0.0.1</version>
<description>Provides an interface between diff_drive_controller and an Arduino.</description>
<maintainer email="josh.newans@gmail.com">Josh Newans</maintainer>
<author email="josh.newans@gmail.com">Josh Newans</author>
<license>BSD</license>
<!-- <url type="website">http://wiki.ros.org/diffdrive_arduino</url> -->
<url type="bugtracker">https://github.com/joshnewans/diffdrive_arduino/issues</url>
<url type="repository">https://github.com/joshnewans/diffdrive_arduino</url>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>hardware_interface</depend>
<depend>controller_manager</depend>
<depend>pluginlib</depend>
<depend>rclcpp</depend>
<depend>serial</depend>
<depend>rclcpp_lifecycle</depend>
<!-- <depend>rclcpp_lifecyle</depend> -->
<test_depend>ament_add_gmock</test_depend>
<test_depend>hardware_interface</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

9
src/diffdrive_arduino/robot_hardware.xml Normal file
View File

@ -0,0 +1,9 @@
<library path="diffdrive_arduino">
<class name="diffdrive_arduino/DiffDriveArduino"
type="diffdrive_arduino::DiffDriveArduino"
base_class_type="hardware_interface::SystemInterface">
<description>
Hardware Interface for Differential Drive Arduino controller.
</description>
</class>
</library>

BIN
src/diffdrive_arduino/src/.DS_Store vendored Normal file
View File

Binary file not shown.

169
src/diffdrive_arduino/src/arduino_comms.cpp Normal file
View File

@ -0,0 +1,169 @@
#include "diffdrive_arduino/arduino_comms.h"
// #include <ros/console.h>
#include <rclcpp/rclcpp.hpp>
#include <sstream>
#include <cstdlib>
#include <arpa/inet.h>
#include <vector>
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<uint8_t> payload = {DRIVE_ENCODER};
std::vector<uint8_t> 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<uint8_t> 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<uint8_t> 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<uint8_t> 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<double*>(&buffer[0]);
safeRead(buffer, payload_size);
std::vector<uint8_t> 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<uint8_t> left_speed_bytes(payload.begin(), payload.begin() + 4);
std::vector<uint8_t> 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<float*>(&left_speed_bytes[0]);
float right_speed_float = *reinterpret_cast<float*>(&right_speed_bytes[0]);
left_speed = left_speed_float;
right_speed = right_speed_float;
}
}
void ArduinoComms::safeRead(std::vector<uint8_t> &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<uint8_t> ArduinoComms::marshalling(uint16_t number)
{
number = htons(number);
uint8_t array[2];
array[0]=number & 0xff;
array[1]=(number >> 8);
std::vector<uint8_t> vec(array, array + sizeof array / sizeof array[0]);
return vec;
}
std::vector<uint8_t> 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<uint8_t> vec(array, array + sizeof array / sizeof array[0]);
return vec;
}
std::vector<uint8_t> ArduinoComms::marshalling(float number)
{
uint32_t int_interpretation = reinterpret_cast<uint32_t &>( number );
union {
float f;
uint8_t b[4];
} u;
u.f = number;
std::vector<uint8_t> vec(u.b, u.b + sizeof u.b / sizeof u.b[0]);
std::reverse(vec.begin(), vec.end());
return vec;
}
const std::vector<uint8_t> ArduinoComms::buildMsg(uint8_t frame_type, uint8_t frame_component, long time, std::vector<uint8_t> payload)
{
std::vector<uint8_t> 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<uint8_t> &msg_to_send, bool print_output)
{
serial_conn_.write(msg_to_send);
}

149
src/diffdrive_arduino/src/diffdrive_arduino.cpp Normal file
View File

@ -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<hardware_interface::StateInterface> DiffDriveArduino::export_state_interfaces()
{
// We need to set up a position and a velocity interface for each wheel
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> DiffDriveArduino::export_command_interfaces()
{
// We need to set up a velocity command interface for each wheel
std::vector<hardware_interface::CommandInterface> 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<double> 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
)

39
src/diffdrive_arduino/src/diffdrive_robot.cpp Normal file
View File

@ -0,0 +1,39 @@
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#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();
}
}

117
src/diffdrive_arduino/src/fake_robot.cpp Normal file
View File

@ -0,0 +1,117 @@
#include "diffdrive_arduino/fake_robot.h"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include <string>
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<hardware_interface::StateInterface> FakeRobot::export_state_interfaces()
{
// We need to set up a position and a velocity interface for each wheel
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> FakeRobot::export_command_interfaces()
{
// We need to set up a velocity command interface for each wheel
std::vector<hardware_interface::CommandInterface> 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<double> 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
)

21
src/diffdrive_arduino/src/wheel.cpp Normal file
View File

@ -0,0 +1,21 @@
#include "diffdrive_arduino/wheel.h"
#include <cmath>
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;
}