Migrate to a new physical robot
Uses the mecanum controller properly across physical and virtual There is a timing issue with i2c which is why the control update is limited to 10hz The sonar and LED's arent yet working, this will come soon.
This commit is contained in:
@@ -0,0 +1,31 @@
|
||||
cmake_minimum_required(VERSION 3.20)
|
||||
project(raspbot_v2_control)
|
||||
|
||||
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)
|
||||
|
||||
install(
|
||||
DIRECTORY config launch
|
||||
DESTINATION share/${PROJECT_NAME}/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,17 @@
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
||||
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
@@ -0,0 +1,46 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 10 # Hz — hardware-limited by STM32 I2C clock-stretch (~22 ms/write × 4 motors)
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
mecanum_drive_controller:
|
||||
type: mecanum_drive_controller/MecanumDriveController
|
||||
|
||||
pan_tilt_controller:
|
||||
type: forward_command_controller/ForwardCommandController
|
||||
|
||||
# ── Mecanum drive controller ─────────────────────────────────────────────────
|
||||
mecanum_drive_controller:
|
||||
ros__parameters:
|
||||
front_left_wheel_command_joint_name: joint_wheel_front_left
|
||||
front_right_wheel_command_joint_name: joint_wheel_front_right
|
||||
rear_left_wheel_command_joint_name: joint_wheel_rear_left
|
||||
rear_right_wheel_command_joint_name: joint_wheel_rear_right
|
||||
|
||||
# Geometry (metres) — derived from raspbot_v2_params.urdf.xacro:
|
||||
# wx = base_length/2 - wheel_radius = 0.09 - 0.03 = 0.06 m (front-rear)
|
||||
# wy = base_width/2 + wheel_width/2 = 0.045 + 0.015 = 0.06 m (left-right)
|
||||
# sum_of_robot_center_projection = wx + wy = 0.12
|
||||
kinematics:
|
||||
wheels_radius: 0.03
|
||||
sum_of_robot_center_projection_on_X_Y_axis: 0.12
|
||||
|
||||
# Odometry
|
||||
enable_odom_tf: true
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_footprint
|
||||
|
||||
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
|
||||
twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3]
|
||||
|
||||
# ── Pan / tilt position controller ───────────────────────────────────────────
|
||||
# Accepts std_msgs/Float64MultiArray — index 0 = pan (rad), index 1 = tilt (rad)
|
||||
# Topic: /pan_tilt_controller/commands
|
||||
pan_tilt_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- pan
|
||||
- tilt
|
||||
interface_name: position
|
||||
@@ -0,0 +1,3 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
use_sim_time: true
|
||||
@@ -0,0 +1,119 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, RegisterEventHandler,IncludeLaunchDescription
|
||||
from launch.conditions import IfCondition,UnlessCondition
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
|
||||
|
||||
from launch_ros.actions import Node, PushRosNamespace
|
||||
from launch_ros.parameter_descriptions import ParameterValue
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
def generate_launch_description():
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
use_hardware = LaunchConfiguration('use_hardware')
|
||||
namespace = LaunchConfiguration('namespace')
|
||||
enable_camera = LaunchConfiguration('enable_camera')
|
||||
|
||||
declared_args = [
|
||||
DeclareLaunchArgument(
|
||||
'namespace', default_value='',
|
||||
description='ROS namespace for all robot nodes (e.g. "robot1").'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation time.'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'use_hardware', default_value='false',
|
||||
description='Load the real I2C hardware interface plugin (set true on physical robot).'
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
'enable_camera', default_value='false',
|
||||
description='Enable the camera node.'
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
robot_controllers = PathJoinSubstitution([
|
||||
FindPackageShare('raspbot_v2_control'), 'config', 'controllers.yaml'
|
||||
])
|
||||
|
||||
robot_description_content = Command([
|
||||
PathJoinSubstitution([FindExecutable(name='xacro')]),
|
||||
' ',
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('raspbot_v2_description'), 'urdf', 'raspbot_v2.urdf.xacro'
|
||||
]),
|
||||
' use_sim_time:=', use_sim_time,
|
||||
' use_hardware:=', use_hardware,
|
||||
' controller_config:=', robot_controllers,
|
||||
])
|
||||
robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
|
||||
|
||||
control_node = Node(
|
||||
package='controller_manager',
|
||||
executable='ros2_control_node',
|
||||
parameters=[robot_description, robot_controllers],
|
||||
output='both',
|
||||
remappings=[
|
||||
# ("~/robot_description", "robot_description"),
|
||||
("mecanum_drive_controller/reference", "/cmd_vel"),
|
||||
("mecanum_drive_controller/odometry", "/odom"),
|
||||
("mecanum_drive_controller/tf_odometry", "/tf"),
|
||||
],
|
||||
condition=UnlessCondition(use_sim_time),
|
||||
)
|
||||
|
||||
robot_state_publisher_node = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
output='both',
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['joint_state_broadcaster', '--controller-manager',
|
||||
PathJoinSubstitution(["/", namespace, "controller_manager"]),
|
||||
'--controller-manager-timeout', '30',
|
||||
],
|
||||
)
|
||||
|
||||
robot_controller_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['mecanum_drive_controller', '--controller-manager',
|
||||
PathJoinSubstitution(["/", namespace, "controller_manager"]),
|
||||
'--controller-manager-timeout', '30',
|
||||
]
|
||||
)
|
||||
|
||||
# Delay starting the robot controller until the joint state broadcaster has finished starting up.
|
||||
delay_robot_controller_after_joint_state_broadcaster = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[robot_controller_spawner],
|
||||
),
|
||||
)
|
||||
|
||||
pan_tilt_controller_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['pan_tilt_controller', '--controller-manager',
|
||||
PathJoinSubstitution(["/", namespace, "controller_manager"]),
|
||||
'--controller-manager-timeout', '30',
|
||||
],
|
||||
condition=IfCondition(enable_camera),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
*declared_args,
|
||||
PushRosNamespace(namespace),
|
||||
control_node,
|
||||
robot_state_publisher_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_after_joint_state_broadcaster,
|
||||
pan_tilt_controller_spawner,
|
||||
])
|
||||
@@ -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>raspbot_v2_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="matthew@thespencers.me.uk">Matt Spencer</maintainer>
|
||||
<license>MIT</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>
|
||||
Reference in New Issue
Block a user