init
This commit is contained in:
commit
0c69e889fb
96
CMakeLists.txt
Normal file
96
CMakeLists.txt
Normal file
@ -0,0 +1,96 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
project(cyphal_hardware_interface LANGUAGES CXX)
|
||||
|
||||
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
|
||||
add_compile_options(-Wall -Wextra)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
set(THIS_PACKAGE_INCLUDE_DEPENDS
|
||||
hardware_interface
|
||||
pluginlib
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
)
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
find_package(${Dependency} REQUIRED)
|
||||
endforeach()
|
||||
|
||||
find_package(simplesync)
|
||||
# message(${simplesync_INCLUDE_DIR})
|
||||
|
||||
## COMPILE
|
||||
add_library(
|
||||
cyphal_hardware_interface
|
||||
SHARED
|
||||
hardware/simplesync_system.cpp
|
||||
)
|
||||
|
||||
function(get_all_targets var)
|
||||
set(targets)
|
||||
get_all_targets_recursive(targets ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
set(${var} ${targets} PARENT_SCOPE)
|
||||
endfunction()
|
||||
|
||||
macro(get_all_targets_recursive targets dir)
|
||||
get_property(subdirectories DIRECTORY ${dir} PROPERTY SUBDIRECTORIES)
|
||||
foreach(subdir ${subdirectories})
|
||||
get_all_targets_recursive(${targets} ${subdir})
|
||||
endforeach()
|
||||
|
||||
get_property(current_targets DIRECTORY ${dir} PROPERTY BUILDSYSTEM_TARGETS)
|
||||
list(APPEND ${targets} ${current_targets})
|
||||
endmacro()
|
||||
|
||||
target_compile_features(cyphal_hardware_interface PUBLIC cxx_std_17)
|
||||
target_include_directories(cyphal_hardware_interface PUBLIC
|
||||
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
|
||||
$<INSTALL_INTERFACE:include/cyphal_hardware_interface>
|
||||
${simplesync_INCLUDE_DIR}
|
||||
)
|
||||
ament_target_dependencies(
|
||||
cyphal_hardware_interface PUBLIC
|
||||
${THIS_PACKAGE_INCLUDE_DEPENDS}
|
||||
)
|
||||
|
||||
# Causes the visibility macros to use dllexport rather than dllimport,
|
||||
# which is appropriate when building the dll but not consuming it.
|
||||
target_compile_definitions(${PROJECT_NAME} PRIVATE "CYPHAL_HARDWARE_INTERFACE_BUILDING_DLL")
|
||||
|
||||
# Export hardware plugins
|
||||
pluginlib_export_plugin_description_file(hardware_interface cyphal_hardware_interface.xml)
|
||||
|
||||
# INSTALL
|
||||
install(
|
||||
DIRECTORY hardware/include/
|
||||
DESTINATION include/cyphal_hardware_interface
|
||||
)
|
||||
install(
|
||||
DIRECTORY description/launch description/ros2_control description/urdf
|
||||
DESTINATION share/cyphal_hardware_interface
|
||||
)
|
||||
install(
|
||||
DIRECTORY bringup/launch bringup/config
|
||||
DESTINATION share/cyphal_hardware_interface
|
||||
)
|
||||
install(TARGETS cyphal_hardware_interface
|
||||
EXPORT export_cyphal_hardware_interface
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_cmake_pytest REQUIRED)
|
||||
|
||||
ament_add_pytest_test(example_2_urdf_xacro test/test_urdf_xacro.py)
|
||||
ament_add_pytest_test(view_example_2_launch test/test_view_robot_launch.py)
|
||||
endif()
|
||||
|
||||
## EXPORTS
|
||||
ament_export_targets(export_cyphal_hardware_interface HAS_LIBRARY_TARGET)
|
||||
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
|
||||
ament_package()
|
||||
12
README.md
Normal file
12
README.md
Normal file
@ -0,0 +1,12 @@
|
||||
# cyphal_hardware_interface
|
||||
|
||||
|
||||
<!-- ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true --remap cmd_vel:=staubi_base_controller/cmd_vel -->
|
||||
<!-- ros2 launch teleop_twist_joy teleop-launch.py joy_vel:=staubi_base_controller/cmd_vel -->
|
||||
<!-- ros2 topic pub /vacuum_controller/commands std_msgs/msg/Float64MultiArray "data: [1]" -->
|
||||
|
||||
# Joystick in podman
|
||||
```
|
||||
sudo podman run -it --network host --volume="/home/nils/development/ros2_ws:/ros2_ws" --privileged osrf/ros:rolling-desktop-full bash -c "ros2 run joy joy_node"
|
||||
```
|
||||
|
||||
77
bringup/config/staubi_controllers.yaml
Normal file
77
bringup/config/staubi_controllers.yaml
Normal file
@ -0,0 +1,77 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 60 # Hz
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
staubi_base_controller:
|
||||
type: diff_drive_controller/DiffDriveController
|
||||
|
||||
vacuum_controller:
|
||||
# type: forward_command_controller/ForwardCommandController
|
||||
type: joy_forward_controller/JoyForwardController
|
||||
|
||||
vacuum_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- vacuum
|
||||
interface_name: power
|
||||
button_number: 9
|
||||
button_toggle: true
|
||||
|
||||
staubi_base_controller:
|
||||
ros__parameters:
|
||||
left_wheel_names: ["left_wheel_joint"]
|
||||
right_wheel_names: ["right_wheel_joint"]
|
||||
|
||||
use_stamped_vel: false
|
||||
|
||||
wheel_separation: 0.222
|
||||
#wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
|
||||
wheel_radius: 0.0355
|
||||
|
||||
wheel_separation_multiplier: 1.0
|
||||
left_wheel_radius_multiplier: 1.0
|
||||
right_wheel_radius_multiplier: 1.0
|
||||
|
||||
publish_rate: 60.0
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_link
|
||||
pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
|
||||
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
|
||||
|
||||
open_loop: false
|
||||
# open_loop: true
|
||||
position_feedback: true
|
||||
enable_odom_tf: true
|
||||
|
||||
cmd_vel_timeout: 0.5
|
||||
#publish_limited_velocity: true
|
||||
#velocity_rolling_window_size: 10
|
||||
|
||||
# Velocity and acceleration limits
|
||||
# Whenever a min_* is unspecified, default to -max_*
|
||||
linear.x.has_velocity_limits: true
|
||||
# linear.x.has_velocity_limits: false
|
||||
linear.x.has_acceleration_limits: true
|
||||
# linear.x.has_acceleration_limits: false
|
||||
# linear.x.has_jerk_limits: true
|
||||
linear.x.has_jerk_limits: false
|
||||
linear.x.max_velocity: 1.3
|
||||
linear.x.min_velocity: -0.3
|
||||
# linear.x.max_acceleration: 10.0
|
||||
linear.x.max_acceleration: 1.4
|
||||
# linear.x.max_acceleration: 10.0
|
||||
# linear.x.max_jerk: 1.0
|
||||
# linear.x.min_jerk: 0.0
|
||||
|
||||
angular.z.has_velocity_limits: true
|
||||
angular.z.has_acceleration_limits: true
|
||||
angular.z.has_jerk_limits: false
|
||||
angular.z.max_velocity: 7.0
|
||||
angular.z.min_velocity: -7.0
|
||||
angular.z.max_acceleration: 3.0
|
||||
angular.z.min_acceleration: -3.0
|
||||
angular.z.max_jerk: 0.0
|
||||
angular.z.min_jerk: 0.0
|
||||
445
bringup/config/staubi_nav2.yaml
Normal file
445
bringup/config/staubi_nav2.yaml
Normal file
@ -0,0 +1,445 @@
|
||||
amcl:
|
||||
ros__parameters:
|
||||
alpha1: 0.2
|
||||
alpha2: 0.2
|
||||
alpha3: 0.2
|
||||
alpha4: 0.2
|
||||
alpha5: 0.2
|
||||
base_frame_id: "base_link"
|
||||
beam_skip_distance: 0.5
|
||||
beam_skip_error_threshold: 0.9
|
||||
beam_skip_threshold: 0.3
|
||||
do_beamskip: false
|
||||
global_frame_id: "map"
|
||||
lambda_short: 0.1
|
||||
laser_likelihood_max_dist: 2.0
|
||||
laser_max_range: 100.0
|
||||
laser_min_range: -1.0
|
||||
laser_model_type: "likelihood_field"
|
||||
max_beams: 60
|
||||
max_particles: 2000
|
||||
min_particles: 500
|
||||
odom_frame_id: "odom"
|
||||
pf_err: 0.05
|
||||
pf_z: 0.99
|
||||
recovery_alpha_fast: 0.0
|
||||
recovery_alpha_slow: 0.0
|
||||
resample_interval: 1
|
||||
robot_model_type: "nav2_amcl::DifferentialMotionModel"
|
||||
save_pose_rate: 0.5
|
||||
sigma_hit: 0.2
|
||||
tf_broadcast: true
|
||||
transform_tolerance: 1.0
|
||||
update_min_a: 0.2
|
||||
update_min_d: 0.25
|
||||
z_hit: 0.5
|
||||
z_max: 0.05
|
||||
z_rand: 0.5
|
||||
z_short: 0.05
|
||||
scan_topic: scan
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
action_server_result_timeout: 900.0
|
||||
navigators: ["navigate_to_pose", "navigate_through_poses"]
|
||||
navigate_to_pose:
|
||||
plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
|
||||
navigate_through_poses:
|
||||
plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
|
||||
# plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
|
||||
# Built-in plugins are added automatically
|
||||
# plugin_lib_names: []
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
error_code_names:
|
||||
- compute_path_error_code
|
||||
- follow_path_error_code
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugins: ["progress_checker"]
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
use_realtime_priority: false
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
FollowPath:
|
||||
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
|
||||
desired_linear_vel: 0.5
|
||||
lookahead_dist: 0.6
|
||||
min_lookahead_dist: 0.3
|
||||
max_lookahead_dist: 0.9
|
||||
lookahead_time: 1.5
|
||||
rotate_to_heading_angular_vel: 1.8
|
||||
transform_tolerance: 0.1
|
||||
use_velocity_scaled_lookahead_dist: false
|
||||
min_approach_linear_velocity: 0.05
|
||||
approach_velocity_scaling_dist: 0.6
|
||||
use_collision_detection: true
|
||||
max_allowed_time_to_collision_up_to_carrot: 1.0
|
||||
use_regulated_linear_velocity_scaling: true
|
||||
use_fixed_curvature_lookahead: false
|
||||
curvature_lookahead_dist: 0.25
|
||||
use_cost_regulated_linear_velocity_scaling: false
|
||||
regulated_linear_scaling_min_radius: 0.9
|
||||
regulated_linear_scaling_min_speed: 0.25
|
||||
use_rotate_to_heading: true
|
||||
allow_reversing: false
|
||||
rotate_to_heading_min_angle: 0.785
|
||||
max_angular_accel: 3.2
|
||||
max_robot_pose_search_dist: 10.0
|
||||
|
||||
# plugin: "nav2_mppi_controller::MPPIController"
|
||||
# time_steps: 56
|
||||
# model_dt: 0.05
|
||||
# batch_size: 2000
|
||||
# vx_std: 0.2
|
||||
# vy_std: 0.2
|
||||
# wz_std: 0.4
|
||||
# vx_max: 0.5
|
||||
# vx_min: -0.35
|
||||
# vy_max: 0.5
|
||||
# wz_max: 1.9
|
||||
# iteration_count: 1
|
||||
# prune_distance: 1.7
|
||||
# transform_tolerance: 0.1
|
||||
# temperature: 0.3
|
||||
# gamma: 0.015
|
||||
# motion_model: "DiffDrive"
|
||||
# visualize: true
|
||||
# regenerate_noises: true
|
||||
# TrajectoryVisualizer:
|
||||
# trajectory_step: 5
|
||||
# time_step: 3
|
||||
# AckermannConstraints:
|
||||
# min_turning_r: 0.2
|
||||
# critics: [
|
||||
# "ConstraintCritic", "CostCritic", "GoalCritic",
|
||||
# "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
|
||||
# "PathAngleCritic", "PreferForwardCritic"]
|
||||
# ConstraintCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 4.0
|
||||
# GoalCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 5.0
|
||||
# threshold_to_consider: 1.4
|
||||
# GoalAngleCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 3.0
|
||||
# threshold_to_consider: 0.5
|
||||
# PreferForwardCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 5.0
|
||||
# threshold_to_consider: 0.5
|
||||
# CostCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 3.81
|
||||
# critical_cost: 300.0
|
||||
# consider_footprint: true
|
||||
# collision_cost: 1000000.0
|
||||
# near_goal_distance: 1.0
|
||||
# trajectory_point_step: 2
|
||||
# PathAlignCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 14.0
|
||||
# max_path_occupancy_ratio: 0.05
|
||||
# trajectory_point_step: 4
|
||||
# threshold_to_consider: 0.5
|
||||
# offset_from_furthest: 20
|
||||
# use_path_orientations: false
|
||||
# PathFollowCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 5.0
|
||||
# offset_from_furthest: 5
|
||||
# threshold_to_consider: 1.4
|
||||
# PathAngleCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 2.0
|
||||
# offset_from_furthest: 4
|
||||
# threshold_to_consider: 0.5
|
||||
# max_angle_to_furthest: 1.0
|
||||
# mode: 0
|
||||
|
||||
# plugin: "dwb_core::DWBLocalPlanner"
|
||||
# debug_trajectory_details: True
|
||||
# min_vel_x: 0.0
|
||||
# min_vel_y: 0.0
|
||||
# max_vel_x: 0.26
|
||||
# max_vel_y: 0.0
|
||||
# max_vel_theta: 1.0
|
||||
# min_speed_xy: 0.0
|
||||
# max_speed_xy: 0.26
|
||||
# min_speed_theta: 0.0
|
||||
# acc_lim_x: 2.5
|
||||
# acc_lim_y: 0.0
|
||||
# acc_lim_theta: 3.2
|
||||
# decel_lim_x: -2.5
|
||||
# decel_lim_y: 0.0
|
||||
# decel_lim_theta: -3.2
|
||||
# vx_samples: 20
|
||||
# vy_samples: 5
|
||||
# vtheta_samples: 20
|
||||
# sim_time: 1.7
|
||||
# linear_granularity: 0.05
|
||||
# angular_granularity: 0.025
|
||||
# transform_tolerance: 0.2
|
||||
# xy_goal_tolerance: 0.25
|
||||
# trans_stopped_velocity: 0.25
|
||||
# short_circuit_trajectory_evaluation: True
|
||||
# stateful: True
|
||||
# critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
# BaseObstacle.scale: 0.02
|
||||
# PathAlign.scale: 32.0
|
||||
# GoalAlign.scale: 24.0
|
||||
# PathAlign.forward_point_distance: 0.1
|
||||
# GoalAlign.forward_point_distance: 0.1
|
||||
# PathDist.scale: 32.0
|
||||
# GoalDist.scale: 24.0
|
||||
# RotateToGoal.scale: 32.0
|
||||
# RotateToGoal.slowing_factor: 5.0
|
||||
# RotateToGoal.lookahead_time: -1.0
|
||||
# TwirlingCritic:
|
||||
# enabled: true
|
||||
# twirling_cost_power: 1
|
||||
# twirling_cost_weight: 10.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.70
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.7
|
||||
always_send_full_costmap: True
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
|
||||
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
|
||||
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
|
||||
# map_server:
|
||||
# ros__parameters:
|
||||
# yaml_filename: ""
|
||||
|
||||
map_saver:
|
||||
ros__parameters:
|
||||
save_map_timeout: 5.0
|
||||
free_thresh_default: 0.25
|
||||
occupied_thresh_default: 0.65
|
||||
map_subscribe_transient_local: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner::NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
enable_stamped_cmd_vel: true
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
local_costmap_topic: local_costmap/costmap_raw
|
||||
global_costmap_topic: global_costmap/costmap_raw
|
||||
local_footprint_topic: local_costmap/published_footprint
|
||||
global_footprint_topic: global_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors::Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors::BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors::DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors::Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors::AssistedTeleop"
|
||||
local_frame: odom
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
action_server_result_timeout: 900.0
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.5, 0.0, 2.0]
|
||||
min_velocity: [-0.5, 0.0, -2.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
enable_stamped_cmd_vel: true
|
||||
|
||||
collision_monitor:
|
||||
ros__parameters:
|
||||
base_frame_id: "base_link"
|
||||
odom_frame_id: "odom"
|
||||
cmd_vel_in_topic: "cmd_vel_smoothed"
|
||||
cmd_vel_out_topic: "cmd_vel"
|
||||
state_topic: "collision_monitor_state"
|
||||
transform_tolerance: 0.2
|
||||
source_timeout: 1.0
|
||||
base_shift_correction: True
|
||||
stop_pub_timeout: 2.0
|
||||
enable_stamped_cmd_vel: true
|
||||
# Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
|
||||
# and robot footprint for "approach" action type.
|
||||
polygons: ["FootprintApproach"]
|
||||
FootprintApproach:
|
||||
type: "polygon"
|
||||
action_type: "approach"
|
||||
footprint_topic: "/local_costmap/published_footprint"
|
||||
time_before_collision: 1.2
|
||||
simulation_time_step: 0.1
|
||||
min_points: 6
|
||||
visualize: False
|
||||
enabled: True
|
||||
observation_sources: ["scan"]
|
||||
scan:
|
||||
type: "scan"
|
||||
topic: "scan"
|
||||
min_height: 0.15
|
||||
max_height: 2.0
|
||||
enabled: True
|
||||
253
bringup/launch/staubi.launch.py
Normal file
253
bringup/launch/staubi.launch.py
Normal file
@ -0,0 +1,253 @@
|
||||
# Copyright 2020 ros2_control Development Team
|
||||
#
|
||||
# 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.
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, RegisterEventHandler
|
||||
from launch.conditions import IfCondition
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
|
||||
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
def generate_launch_description():
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gui",
|
||||
default_value="false",
|
||||
description="Start RViz2 automatically with this launch file.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"joy",
|
||||
default_value="true",
|
||||
description="Start joy node with this launch file.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"use_mock_hardware",
|
||||
default_value="false",
|
||||
description="Start robot with mock hardware mirroring command to its states.",
|
||||
)
|
||||
)
|
||||
lidar_serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200')
|
||||
lidar_channel_type = LaunchConfiguration('channel_type', default='serial')
|
||||
lidar_frame_id = LaunchConfiguration('frame_id', default='laser')
|
||||
lidar_serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
|
||||
lidar_inverted = LaunchConfiguration('inverted', default='false')
|
||||
lidar_angle_compensate = LaunchConfiguration('angle_compensate', default='true')
|
||||
lidar_scan_mode = LaunchConfiguration('scan_mode', default='Standard')
|
||||
joy_dev = LaunchConfiguration('joy_dev', default='/dev/input/js0')
|
||||
|
||||
# Initialize Arguments
|
||||
gui = LaunchConfiguration("gui")
|
||||
joy = LaunchConfiguration("joy")
|
||||
use_mock_hardware = LaunchConfiguration("use_mock_hardware")
|
||||
|
||||
# Get URDF via xacro
|
||||
robot_description_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
PathJoinSubstitution(
|
||||
[FindPackageShare("cyphal_hardware_interface"), "urdf", "staubi.urdf.xacro"]
|
||||
),
|
||||
" ",
|
||||
"use_mock_hardware:=",
|
||||
use_mock_hardware,
|
||||
]
|
||||
)
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
robot_controllers = PathJoinSubstitution(
|
||||
[
|
||||
FindPackageShare("cyphal_hardware_interface"),
|
||||
"config",
|
||||
"staubi_controllers.yaml",
|
||||
]
|
||||
)
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("cyphal_hardware_interface"), "staubi/rviz", "staubi.rviz"]
|
||||
)
|
||||
|
||||
|
||||
|
||||
lidar_ld = [
|
||||
DeclareLaunchArgument(
|
||||
'channel_type',
|
||||
default_value=lidar_channel_type,
|
||||
description='Specifying channel type of lidar'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'serial_port',
|
||||
default_value=lidar_serial_port,
|
||||
description='Specifying usb port to connected lidar'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'serial_baudrate',
|
||||
default_value=lidar_serial_baudrate,
|
||||
description='Specifying usb port baudrate to connected lidar'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id',
|
||||
default_value=lidar_frame_id,
|
||||
description='Specifying frame_id of lidar'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'inverted',
|
||||
default_value=lidar_inverted,
|
||||
description='Specifying whether or not to invert scan data'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'angle_compensate',
|
||||
default_value=lidar_angle_compensate,
|
||||
description='Specifying whether or not to enable angle_compensate of scan data'),
|
||||
DeclareLaunchArgument(
|
||||
'scan_mode',
|
||||
default_value=lidar_scan_mode,
|
||||
description='Specifying scan mode of lidar'),
|
||||
|
||||
Node(
|
||||
package='rplidar_ros',
|
||||
executable='rplidar_node',
|
||||
name='rplidar_node',
|
||||
parameters=[{'channel_type':lidar_channel_type,
|
||||
'serial_port': lidar_serial_port,
|
||||
'serial_baudrate': lidar_serial_baudrate,
|
||||
'frame_id': lidar_frame_id,
|
||||
'inverted': lidar_inverted,
|
||||
'angle_compensate': lidar_angle_compensate,
|
||||
'scan_mode': lidar_scan_mode
|
||||
}],
|
||||
output='screen'),]
|
||||
|
||||
control_node = Node(
|
||||
package="controller_manager",
|
||||
executable="ros2_control_node",
|
||||
parameters=[robot_description, robot_controllers],
|
||||
output="both",
|
||||
remappings=[
|
||||
("/vacuum_controller/joy", "/joy"),
|
||||
("/staubi_base_controller/cmd_vel", "/cmd_vel_nav"),
|
||||
],
|
||||
)
|
||||
robot_state_pub_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
# remappings=[
|
||||
# ("/diff_drive_controller/cmd_vel_unstamped", "/cmd_vel"),
|
||||
# ],
|
||||
)
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
condition=IfCondition(gui),
|
||||
)
|
||||
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
robot_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["staubi_base_controller", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
vacuum_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["vacuum_controller", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
# Delay rviz start after `joint_state_broadcaster`
|
||||
delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[rviz_node],
|
||||
)
|
||||
)
|
||||
|
||||
# Delay start of robot_controller after `joint_state_broadcaster`
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[robot_controller_spawner],
|
||||
)
|
||||
)
|
||||
|
||||
delay_vacuum_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[vacuum_controller_spawner],
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
|
||||
joy_ld = [
|
||||
DeclareLaunchArgument('joy_vel', default_value='/cmd_vel_nav'),
|
||||
DeclareLaunchArgument('joy_config', default_value='ps3'),
|
||||
DeclareLaunchArgument('joy_dev', default_value=joy_dev),
|
||||
Node(
|
||||
package='joy_linux', executable='joy_linux_node', name='joy_linux_node',
|
||||
parameters=[{
|
||||
'dev': joy_dev,
|
||||
# 'deadzone': 0.3,
|
||||
# 'autorepeat_rate': 20.0,
|
||||
}],
|
||||
condition=IfCondition(joy),
|
||||
),
|
||||
Node(
|
||||
package='teleop_twist_joy', executable='teleop_node',
|
||||
name='teleop_twist_joy_node', parameters=[ {
|
||||
"publish_stamped_twist": True,
|
||||
# "require_enable_button": False,
|
||||
"enable_button": 10,
|
||||
# "scale_linear.x":3.0,
|
||||
"axis_angular.yaw": 2,
|
||||
"axis_linear.x": 3,
|
||||
"scale_linear.x": 0.5,
|
||||
"scale_angular.yaw": 1.6,
|
||||
"scale_linear_turbo.x": 1.8,
|
||||
"scale_angular_turbo.yaw": 2.6,
|
||||
"enable_turbo_button": 7,
|
||||
"frame": "base_link",
|
||||
}],
|
||||
# condition=IfCondition(joystick),
|
||||
remappings={('/cmd_vel', LaunchConfiguration('joy_vel'))},
|
||||
),
|
||||
]
|
||||
|
||||
nodes = [
|
||||
control_node,
|
||||
robot_state_pub_node,
|
||||
joint_state_broadcaster_spawner,
|
||||
delay_rviz_after_joint_state_broadcaster_spawner,
|
||||
delay_robot_controller_spawner_after_joint_state_broadcaster_spawner,
|
||||
delay_vacuum_controller_spawner_after_joint_state_broadcaster_spawner,
|
||||
] + joy_ld + lidar_ld
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes)
|
||||
9
cyphal_hardware_interface.xml
Normal file
9
cyphal_hardware_interface.xml
Normal file
@ -0,0 +1,9 @@
|
||||
<library path="cyphal_hardware_interface">
|
||||
<class name="cyphal_hardware_interface/cyphalSystemHardware"
|
||||
type="cyphal_hardware_interface::cyphalSystemHardware"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
The ros2_control cyphal_hardware_interface using a system hardware interface-type.
|
||||
</description>
|
||||
</class>
|
||||
</library>
|
||||
111
description/launch/view_robot.launch.py
Normal file
111
description/launch/view_robot.launch.py
Normal file
@ -0,0 +1,111 @@
|
||||
# Copyright 2021 Stogl Robotics Consulting UG (haftungsbeschränkt)
|
||||
#
|
||||
# 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.
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.conditions import IfCondition
|
||||
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
||||
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Declare arguments
|
||||
declared_arguments = []
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"description_package",
|
||||
default_value="ros2_control_demo_description",
|
||||
description="Description package with robot URDF/xacro files. Usually the argument \
|
||||
is not set, it enables use of a custom description.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"description_file",
|
||||
default_value="staubi.urdf.xacro",
|
||||
description="URDF/XACRO description file with the robot.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"gui",
|
||||
default_value="true",
|
||||
description="Start Rviz2 and Joint State Publisher gui automatically \
|
||||
with this launch file.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"prefix",
|
||||
default_value='""',
|
||||
description="Prefix of the joint names, useful for \
|
||||
multi-robot setup. If changed than also joint names in the controllers' configuration \
|
||||
have to be updated.",
|
||||
)
|
||||
)
|
||||
|
||||
# Initialize Arguments
|
||||
description_package = LaunchConfiguration("description_package")
|
||||
description_file = LaunchConfiguration("description_file")
|
||||
gui = LaunchConfiguration("gui")
|
||||
prefix = LaunchConfiguration("prefix")
|
||||
|
||||
# Get URDF via xacro
|
||||
robot_description_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
PathJoinSubstitution(
|
||||
[FindPackageShare("cyphal_hardware_interface"), "urdf", description_file]
|
||||
),
|
||||
" ",
|
||||
"prefix:=",
|
||||
prefix,
|
||||
]
|
||||
)
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare(description_package), "staubi/rviz", "staubi_view.rviz"]
|
||||
)
|
||||
|
||||
joint_state_publisher_node = Node(
|
||||
package="joint_state_publisher_gui",
|
||||
executable="joint_state_publisher_gui",
|
||||
condition=IfCondition(gui),
|
||||
)
|
||||
robot_state_publisher_node = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
output="both",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
rviz_node = Node(
|
||||
package="rviz2",
|
||||
executable="rviz2",
|
||||
name="rviz2",
|
||||
output="log",
|
||||
arguments=["-d", rviz_config_file],
|
||||
condition=IfCondition(gui),
|
||||
)
|
||||
|
||||
nodes = [
|
||||
joint_state_publisher_node,
|
||||
robot_state_publisher_node,
|
||||
rviz_node,
|
||||
]
|
||||
|
||||
return LaunchDescription(declared_arguments + nodes)
|
||||
63
description/ros2_control/staubi.ros2_control.xacro
Normal file
63
description/ros2_control/staubi.ros2_control.xacro
Normal file
@ -0,0 +1,63 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="staubi_ros2_control" params="name prefix use_mock_hardware">
|
||||
|
||||
<ros2_control name="${name}" type="system">
|
||||
<xacro:unless value="${use_mock_hardware}">
|
||||
<hardware>
|
||||
<plugin>cyphal_hardware_interface/cyphalSystemHardware</plugin>
|
||||
<param name="port">/dev/ttyAMA10</param>
|
||||
<!--param name="port">/dev/ttyACM0</param-->
|
||||
<param name="baud">460800</param>
|
||||
<!--param name="try_next_port_number">false</param-->
|
||||
</hardware>
|
||||
</xacro:unless>
|
||||
<xacro:if value="${use_mock_hardware}">
|
||||
<hardware>
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
<param name="calculate_dynamics">true</param>
|
||||
</hardware>
|
||||
</xacro:if>
|
||||
<sensor name="${prefix}driver">
|
||||
<param name="state:battery_voltage">vcc</param>
|
||||
<param name="factor">0.001</param>
|
||||
<state_interface name="battery_voltage"/>
|
||||
</sensor>
|
||||
<joint name="${prefix}vacuum">
|
||||
<param name="command:power">s</param>
|
||||
<param name="factor">0.1427</param>
|
||||
<command_interface name="power">
|
||||
<param name="min">0</param>
|
||||
<param name="max">1</param>
|
||||
</command_interface>
|
||||
</joint>
|
||||
<joint name="${prefix}left_wheel_joint">
|
||||
<param name="command:velocity">vt0</param>
|
||||
<param name="state:position">a0</param>
|
||||
<param name="state:velocity">v0</param>
|
||||
<param name="factor">0.0001</param>
|
||||
<command_interface name="velocity">
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
</state_interface>
|
||||
<!--state_interface name="velocity">
|
||||
</state_interface-->
|
||||
</joint>
|
||||
<joint name="${prefix}right_wheel_joint">
|
||||
<param name="command:velocity">vt1</param>
|
||||
<param name="state:position">a1</param>
|
||||
<param name="state:velocity">v1</param>
|
||||
<param name="factor">0.0001</param>
|
||||
<command_interface name="velocity">
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
</state_interface>
|
||||
<!--state_interface name="velocity">
|
||||
</state_interface-->
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
40
description/urdf/staubi.materials.xacro
Normal file
40
description/urdf/staubi.materials.xacro
Normal file
@ -0,0 +1,40 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copied from ROS1 example:
|
||||
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/materials.xacro
|
||||
-->
|
||||
<robot>
|
||||
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="grey">
|
||||
<color rgba="0.2 0.2 0.2 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="orange">
|
||||
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="brown">
|
||||
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
|
||||
</robot>
|
||||
20
description/urdf/staubi.urdf.xacro
Normal file
20
description/urdf/staubi.urdf.xacro
Normal file
@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
<!-- Basic differential drive mobile base -->
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="staubi_robot">
|
||||
<xacro:arg name="prefix" default="" />
|
||||
<xacro:arg name="use_mock_hardware" default="false" />
|
||||
|
||||
<xacro:include filename="$(find cyphal_hardware_interface)/urdf/staubi_description.urdf.xacro" />
|
||||
|
||||
<!-- Import Rviz colors -->
|
||||
<xacro:include filename="$(find cyphal_hardware_interface)/urdf/staubi.materials.xacro" />
|
||||
|
||||
<!-- Import staubi ros2_control description -->
|
||||
<xacro:include filename="$(find cyphal_hardware_interface)/ros2_control/staubi.ros2_control.xacro" />
|
||||
|
||||
<xacro:staubi prefix="$(arg prefix)" />
|
||||
|
||||
<xacro:staubi_ros2_control
|
||||
name="Staubi" prefix="$(arg prefix)" use_mock_hardware="$(arg use_mock_hardware)"/>
|
||||
|
||||
</robot>
|
||||
153
description/urdf/staubi_description.urdf.xacro
Normal file
153
description/urdf/staubi_description.urdf.xacro
Normal file
@ -0,0 +1,153 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="staubi" params="prefix">
|
||||
|
||||
<!-- Constants for robot dimensions -->
|
||||
<xacro:property name="PI" value="3.1415926535897931"/>
|
||||
<xacro:property name="base_mass" value="0.3" /> <!-- arbitrary value for base mass -->
|
||||
<xacro:property name="base_width" value="0.345" />
|
||||
<xacro:property name="base_height" value="0.07" />
|
||||
<xacro:property name="floor_clearance" value="0.02" />
|
||||
<xacro:property name="wheel_mass" value="0.3" /> <!-- arbitrary value for wheel mass -->
|
||||
<xacro:property name="wheel_len" value="0.015" />
|
||||
<xacro:property name="wheel_radius" value="${0.071 / 2}" />
|
||||
<xacro:property name="wheel_distance" value="${0.222}" />
|
||||
<xacro:property name="caster_wheel_mass" value="0.1" /> <!-- arbitrary value for caster wheel mass -->
|
||||
<xacro:property name="caster_wheel_radius" value="0.030" />
|
||||
<xacro:property name="z_offset" value="-${base_height/2}" /> <!-- Space btw top of beam and the each joint -->
|
||||
|
||||
<!-- Base Link -->
|
||||
<link name="${prefix}base_link">
|
||||
<collision>
|
||||
<origin xyz="0 0 ${floor_clearance+base_height/2}" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${base_height}" radius="${base_width/2}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 ${floor_clearance+base_height/2}" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${base_height}" radius="${base_width/2}"/>
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
<visual>
|
||||
<origin xyz="${base_width/2 - 0.02} 0 ${floor_clearance+base_height+0.005}" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.01" radius="0.015"/>
|
||||
</geometry>
|
||||
<material name="red"/>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}left_wheel_joint" type="continuous">
|
||||
<parent link="${prefix}base_link"/>
|
||||
<child link="${prefix}left_wheel"/>
|
||||
<origin xyz="0 ${wheel_distance/2} ${wheel_radius}" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.2"/>
|
||||
<limit effort="100" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<!-- left wheel Link -->
|
||||
<link name="${prefix}left_wheel">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_len}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_len}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}right_wheel_joint" type="continuous">
|
||||
<parent link="${prefix}base_link"/>
|
||||
<child link="${prefix}right_wheel"/>
|
||||
<origin xyz="0 -${wheel_distance/2} ${wheel_radius}" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<dynamics damping="0.2"/>
|
||||
<limit effort="100" velocity="1.0"/>
|
||||
</joint>
|
||||
|
||||
<!-- right wheel Link -->
|
||||
<link name="${prefix}right_wheel">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_len}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_len}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}caster_frontal_wheel_joint" type="fixed">
|
||||
<parent link="${prefix}base_link"/>
|
||||
<child link="${prefix}caster_frontal_wheel"/>
|
||||
<origin xyz="${base_width/2 - caster_wheel_radius} 0 ${caster_wheel_radius}" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<!-- caster frontal wheel Link -->
|
||||
<link name="${prefix}caster_frontal_wheel">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${caster_wheel_radius}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="${caster_wheel_radius}"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="${prefix}laser_joint" type="fixed">
|
||||
<parent link="${prefix}base_link"/>
|
||||
<child link="${prefix}laser"/>
|
||||
<origin xyz="0.025 0.025 ${floor_clearance+base_height+0.01}" rpy="0 0 0.66939"/>
|
||||
</joint>
|
||||
|
||||
<!-- laser Link -->
|
||||
<link name="${prefix}laser">
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.03" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.02" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
</link>
|
||||
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
104
hardware/include/cyphal_hardware_interface/simplesync_system.hpp
Normal file
104
hardware/include/cyphal_hardware_interface/simplesync_system.hpp
Normal file
@ -0,0 +1,104 @@
|
||||
// Copyright 2021 ros2_control Development Team
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef CYPHAL_HARDWARE_INTERFACE__SIMPLESYNC_SYSTEM_HPP_
|
||||
#define CYPHAL_HARDWARE_INTERFACE__SIMPLESYNC_SYSTEM_HPP_
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "hardware_interface/handle.hpp"
|
||||
#include "hardware_interface/hardware_info.hpp"
|
||||
#include "hardware_interface/system_interface.hpp"
|
||||
#include "hardware_interface/types/hardware_interface_return_values.hpp"
|
||||
#include "rclcpp/clock.hpp"
|
||||
#include "rclcpp/duration.hpp"
|
||||
#include "rclcpp/macros.hpp"
|
||||
#include "rclcpp/time.hpp"
|
||||
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
|
||||
#include "rclcpp_lifecycle/state.hpp"
|
||||
|
||||
#include "cyphal_hardware_interface/visibility_control.h"
|
||||
|
||||
#include "simplesync.hpp"
|
||||
|
||||
namespace cyphal_hardware_interface {
|
||||
class cyphalSystemHardware : public hardware_interface::SystemInterface {
|
||||
public:
|
||||
RCLCPP_SHARED_PTR_DEFINITIONS(cyphalSystemHardware);
|
||||
|
||||
constexpr static std::string_view default_serial_port_ = "/dev/ttyACM0";
|
||||
|
||||
std::string serial_port_;
|
||||
bool try_next_port_number;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
virtual ~cyphalSystemHardware();
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
hardware_interface::CallbackReturn
|
||||
on_init(const hardware_interface::HardwareInfo &info) override;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
std::vector<hardware_interface::StateInterface>
|
||||
export_state_interfaces() override;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
std::vector<hardware_interface::CommandInterface>
|
||||
export_command_interfaces() override;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
hardware_interface::CallbackReturn
|
||||
on_configure(const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
hardware_interface::CallbackReturn
|
||||
on_activate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
hardware_interface::CallbackReturn
|
||||
on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
hardware_interface::return_type read(const rclcpp::Time &time,
|
||||
const rclcpp::Duration &period) override;
|
||||
|
||||
CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
hardware_interface::return_type
|
||||
write(const rclcpp::Time &time, const rclcpp::Duration &period) override;
|
||||
|
||||
private:
|
||||
struct interface_params {
|
||||
double value;
|
||||
double factor;
|
||||
std::string joint;
|
||||
bool command;
|
||||
std::string control_interface;
|
||||
};
|
||||
|
||||
int serial_port_handle;
|
||||
|
||||
std::map<std::string, struct interface_params> interfaces_map;
|
||||
typedef simplesync::cyphal<
|
||||
std::chrono::time_point<std::chrono::steady_clock>,
|
||||
std::chrono::nanoseconds, 1024, true>
|
||||
cyphalChrono;
|
||||
std::unique_ptr<cyphalChrono> sync;
|
||||
};
|
||||
|
||||
} // namespace cyphal_hardware_interface
|
||||
|
||||
#endif // CYPHAL_HARDWARE_INTERFACE__SIMPLESYNC_SYSTEM_HPP_
|
||||
@ -0,0 +1,56 @@
|
||||
// Copyright 2021 ros2_control Development Team
|
||||
//
|
||||
// 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.
|
||||
|
||||
/* This header must be included by all rclcpp headers which declare symbols
|
||||
* which are defined in the rclcpp library. When not building the rclcpp
|
||||
* library, i.e. when using the headers in other package's code, the contents
|
||||
* of this header change the visibility of certain symbols which the rclcpp
|
||||
* library cannot have, but the consuming code must have inorder to link.
|
||||
*/
|
||||
|
||||
#ifndef CYPHAL_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
|
||||
#define CYPHAL_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
|
||||
|
||||
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
|
||||
// https://gcc.gnu.org/wiki/Visibility
|
||||
|
||||
#if defined _WIN32 || defined __CYGWIN__
|
||||
#ifdef __GNUC__
|
||||
#define CYPHAL_HARDWARE_INTERFACE_EXPORT __attribute__((dllexport))
|
||||
#define CYPHAL_HARDWARE_INTERFACE_IMPORT __attribute__((dllimport))
|
||||
#else
|
||||
#define CYPHAL_HARDWARE_INTERFACE_EXPORT __declspec(dllexport)
|
||||
#define CYPHAL_HARDWARE_INTERFACE_IMPORT __declspec(dllimport)
|
||||
#endif
|
||||
#ifdef CYPHAL_HARDWARE_INTERFACE_BUILDING_DLL
|
||||
#define CYPHAL_HARDWARE_INTERFACE_PUBLIC CYPHAL_HARDWARE_INTERFACE_EXPORT
|
||||
#else
|
||||
#define CYPHAL_HARDWARE_INTERFACE_PUBLIC CYPHAL_HARDWARE_INTERFACE_IMPORT
|
||||
#endif
|
||||
#define CYPHAL_HARDWARE_INTERFACE_PUBLIC_TYPE CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
#define CYPHAL_HARDWARE_INTERFACE_LOCAL
|
||||
#else
|
||||
#define CYPHAL_HARDWARE_INTERFACE_EXPORT __attribute__((visibility("default")))
|
||||
#define CYPHAL_HARDWARE_INTERFACE_IMPORT
|
||||
#if __GNUC__ >= 4
|
||||
#define CYPHAL_HARDWARE_INTERFACE_PUBLIC __attribute__((visibility("default")))
|
||||
#define CYPHAL_HARDWARE_INTERFACE_LOCAL __attribute__((visibility("hidden")))
|
||||
#else
|
||||
#define CYPHAL_HARDWARE_INTERFACE_PUBLIC
|
||||
#define CYPHAL_HARDWARE_INTERFACE_LOCAL
|
||||
#endif
|
||||
#define CYPHAL_HARDWARE_INTERFACE_PUBLIC_TYPE
|
||||
#endif
|
||||
|
||||
#endif // CYPHAL_HARDWARE_INTERFACE__VISIBILITY_CONTROL_H_
|
||||
430
hardware/simplesync_system.cpp
Normal file
430
hardware/simplesync_system.cpp
Normal file
@ -0,0 +1,430 @@
|
||||
#include "cyphal_hardware_interface/simplesync_system.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <hardware_interface/hardware_info.hpp>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "serial.hpp"
|
||||
|
||||
#include "hardware_interface/types/hardware_interface_type_values.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
#include "simplesync.hpp"
|
||||
|
||||
namespace cyphal_hardware_interface {
|
||||
|
||||
cyphalSystemHardware::~cyphalSystemHardware() {
|
||||
if (serial_port_handle) {
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Closing port '%s'", serial_port_.c_str());
|
||||
close(serial_port_handle);
|
||||
}
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn cyphalSystemHardware::on_init(
|
||||
const hardware_interface::HardwareInfo &info) {
|
||||
if (hardware_interface::SystemInterface::on_init(info) !=
|
||||
hardware_interface::CallbackReturn::SUCCESS) {
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
serial_port_ = default_serial_port_;
|
||||
|
||||
if (info_.hardware_parameters.count("port") > 0) {
|
||||
serial_port_ = info_.hardware_parameters.at("port");
|
||||
} else {
|
||||
RCLCPP_WARN(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"No port specified in urdf, using default value");
|
||||
}
|
||||
if (info_.hardware_parameters.count("try_next_port_number") > 0) {
|
||||
std::string try_next_str =
|
||||
info_.hardware_parameters.at("try_next_port_number");
|
||||
try_next_port_number =
|
||||
std::set<std::string>({"True", "true", "1", "Yes", "yes", "Y", "y"})
|
||||
.count(try_next_str) > 0;
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Trying next ports if the one provided is not openable");
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"), "Port set as %s",
|
||||
serial_port_.c_str());
|
||||
|
||||
std::vector<hardware_interface::ComponentInfo> components;
|
||||
components.insert(components.begin(), info_.joints.begin(),
|
||||
info_.joints.end());
|
||||
components.insert(components.begin(), info_.sensors.begin(),
|
||||
info_.sensors.end());
|
||||
components.insert(components.begin(), info_.gpios.begin(), info_.gpios.end());
|
||||
for (const hardware_interface::ComponentInfo &joint : components) {
|
||||
// StaubiSystem has exactly two states and one command interface on each
|
||||
// joint
|
||||
/*if (joint.command_interfaces.size() != 1) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Joint '%s' has %zu command interfaces found. 1 expected.",
|
||||
joint.name.c_str(), joint.command_interfaces.size());
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}*/
|
||||
for (const auto &i : joint.command_interfaces) {
|
||||
std::string param_name = "command:" + i.name;
|
||||
if (joint.parameters.count(param_name) <= 0) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"command interface '%s' of joint/gpio '%s' has no parameter "
|
||||
"'command:%s'.",
|
||||
i.name.c_str(), joint.name.c_str(), i.name.c_str());
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}
|
||||
auto interface_name = joint.parameters.at(param_name);
|
||||
if (interfaces_map.count(interface_name) > 0) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Sync Interface '%s' used twice. FATAL ERROR.",
|
||||
interface_name.c_str());
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}
|
||||
interfaces_map[interface_name] = {
|
||||
std::numeric_limits<double>::quiet_NaN(),
|
||||
(joint.parameters.count("factor") > 0)
|
||||
? std::stod(joint.parameters.at("factor"))
|
||||
: 1.0,
|
||||
joint.name, true, i.name};
|
||||
}
|
||||
|
||||
for (const auto &i : joint.state_interfaces) {
|
||||
std::string param_name = "state:" + i.name;
|
||||
if (joint.parameters.count(param_name) <= 0) {
|
||||
RCLCPP_FATAL(
|
||||
rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"state interface '%s' of joint/sensor/gpio '%s' has no parameter "
|
||||
"'state:%s'.",
|
||||
i.name.c_str(), joint.name.c_str(), i.name.c_str());
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}
|
||||
auto interface_name = joint.parameters.at(param_name);
|
||||
if (interfaces_map.count(interface_name) > 0) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Sync Interface '%s' used twice. FATAL ERROR.",
|
||||
interface_name.c_str());
|
||||
return hardware_interface::CallbackReturn::ERROR;
|
||||
}
|
||||
interfaces_map[interface_name] = {
|
||||
std::numeric_limits<double>::quiet_NaN(),
|
||||
(joint.parameters.count("factor") > 0)
|
||||
? std::stod(joint.parameters.at("factor"))
|
||||
: 1.0,
|
||||
joint.name, false, i.name};
|
||||
}
|
||||
}
|
||||
// for (const auto &[key, value] : interfaces_map) {
|
||||
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
// "Interface %s added (factor: %f).", key.c_str(), value.factor);
|
||||
// }
|
||||
|
||||
return hardware_interface::CallbackReturn::SUCCESS;
|
||||
} // namespace cyphal_hardware_interface
|
||||
|
||||
std::vector<hardware_interface::StateInterface>
|
||||
cyphalSystemHardware::export_state_interfaces() {
|
||||
std::vector<hardware_interface::StateInterface> state_interfaces;
|
||||
for (auto &[key, value] : interfaces_map) {
|
||||
if (!value.command)
|
||||
state_interfaces.emplace_back(hardware_interface::StateInterface(
|
||||
value.joint, value.control_interface, &value.value));
|
||||
}
|
||||
/* for (auto i = 0u; i < info_.joints.size(); i++) {
|
||||
state_interfaces.emplace_back(hardware_interface::StateInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_POSITION,
|
||||
&hw_positions_[i]));
|
||||
state_interfaces.emplace_back(hardware_interface::StateInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
|
||||
&hw_velocities_[i]));
|
||||
}
|
||||
*/
|
||||
return state_interfaces;
|
||||
}
|
||||
|
||||
std::vector<hardware_interface::CommandInterface>
|
||||
cyphalSystemHardware::export_command_interfaces() {
|
||||
std::vector<hardware_interface::CommandInterface> command_interfaces;
|
||||
for (auto &[key, value] : interfaces_map) {
|
||||
if (value.command)
|
||||
command_interfaces.emplace_back(hardware_interface::CommandInterface(
|
||||
value.joint, value.control_interface, &value.value));
|
||||
}
|
||||
/* for (auto i = 0u; i < info_.joints.size(); i++) {
|
||||
command_interfaces.emplace_back(hardware_interface::CommandInterface(
|
||||
info_.joints[i].name, hardware_interface::HW_IF_VELOCITY,
|
||||
&hw_commands_[i]));
|
||||
}
|
||||
*/
|
||||
return command_interfaces;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn cyphalSystemHardware::on_configure(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/) {
|
||||
// const hardware_interface::CallbackReturn result =
|
||||
// can_interface_->connect(can_socket_)
|
||||
// ? hardware_interface::CallbackReturn::SUCCESS
|
||||
// : hardware_interface::CallbackReturn::FAILURE;
|
||||
// Open the serial port. Change device path as needed (currently set to an
|
||||
// standard FTDI USB-UART cable type device)
|
||||
|
||||
serial_port_handle = open_serial(serial_port_);
|
||||
if (!serial_port_handle) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Error opening port '%s': %s", serial_port_.c_str(),
|
||||
strerror(errno));
|
||||
if (try_next_port_number) {
|
||||
for (unsigned int tries = 0; tries < 100; tries += 1) {
|
||||
size_t digits_idx = serial_port_.size() - 1;
|
||||
while (digits_idx > 0 && std::isdigit(serial_port_[digits_idx]))
|
||||
digits_idx -= 1;
|
||||
digits_idx += 1;
|
||||
std::string number_str = serial_port_.substr(digits_idx);
|
||||
unsigned int number = number_str.size() > 0 ? std::stol(number_str) : 0;
|
||||
serial_port_ =
|
||||
serial_port_.substr(0, digits_idx) + std::to_string(number + 1);
|
||||
serial_port_handle = open_serial(serial_port_);
|
||||
if (!serial_port_handle) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Error opening port '%s': %s", serial_port_.c_str(),
|
||||
strerror(errno));
|
||||
} else
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!serial_port_handle)
|
||||
return hardware_interface::CallbackReturn::FAILURE;
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Successfuly opened port '%s'", serial_port_.c_str());
|
||||
|
||||
sync = std::make_unique<cyphalChrono>(
|
||||
[this](uint8_t *buf, unsigned int buf_size) {
|
||||
std::stringstream ss;
|
||||
ss << std::hex;
|
||||
for (unsigned int i = 0; i < buf_size; i += 1)
|
||||
ss << (int)buf[i] << " ";
|
||||
|
||||
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
// "Write to serial: %s", ss.str().c_str());
|
||||
write_serial(this->serial_port_handle, buf, buf_size);
|
||||
return 0;
|
||||
},
|
||||
[]() { return (std::chrono::steady_clock::now()); });
|
||||
|
||||
sync->request_all_interfaces();
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
std::this_thread::sleep_for(100ms); // FIXME: callback
|
||||
uint8_t read_buf[1024];
|
||||
int read_buf_used =
|
||||
read_serial(serial_port_handle, read_buf, sizeof(read_buf));
|
||||
|
||||
sync->handle_stream((uint8_t *)read_buf, read_buf_used);
|
||||
|
||||
for (auto &i : sync->interfaces) {
|
||||
if (!i)
|
||||
continue;
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Sync-interface %s recived.", i->key.c_str());
|
||||
}
|
||||
|
||||
return hardware_interface::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn cyphalSystemHardware::on_activate(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/) {
|
||||
// BEGIN: This part here is for exemplary purposes - Please do not copy to
|
||||
// your production code
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Activating ...please wait...");
|
||||
|
||||
/* for (auto i = 0; i < hw_start_sec_; i++) {
|
||||
rclcpp::sleep_for(std::chrono::seconds(1));
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"%.1f seconds left...", hw_start_sec_ - i);
|
||||
}
|
||||
// END: This part here is for exemplary purposes - Please do not copy to
|
||||
your
|
||||
// production code
|
||||
|
||||
// set some default values
|
||||
for (auto i = 0u; i < hw_positions_.size(); i++) {
|
||||
if (std::isnan(hw_positions_[i])) {
|
||||
hw_positions_[i] = 0;
|
||||
hw_velocities_[i] = 0;
|
||||
hw_commands_[i] = 0;
|
||||
}
|
||||
}
|
||||
*/
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Successfully activated!");
|
||||
|
||||
return hardware_interface::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::CallbackReturn cyphalSystemHardware::on_deactivate(
|
||||
const rclcpp_lifecycle::State & /*previous_state*/) {
|
||||
// BEGIN: This part here is for exemplary purposes - Please do not copy to
|
||||
// your production code
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Deactivating ...please wait...");
|
||||
|
||||
/* for (auto i = 0; i < hw_stop_sec_; i++) {
|
||||
rclcpp::sleep_for(std::chrono::seconds(1));
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"%.1f seconds left...", hw_stop_sec_ - i);
|
||||
}
|
||||
// END: This part here is for exemplary purposes - Please do not copy to
|
||||
your
|
||||
// production code
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Successfully deactivated!");
|
||||
*/
|
||||
return hardware_interface::CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
hardware_interface::return_type
|
||||
cyphalSystemHardware::read(const rclcpp::Time & /*time*/,
|
||||
const rclcpp::Duration & /*period*/) {
|
||||
// BEGIN: This part here is for exemplary purposes - Please do not copy to
|
||||
// your production code
|
||||
/* for (std::size_t i = 0; i < hw_velocities_.size(); i++) {
|
||||
// Simulate Staubi wheels's movement as a first-order system
|
||||
// Update the joint status: this is a revolute joint without any limit.
|
||||
// Simply integrates
|
||||
hw_positions_[i] = hw_positions_[i] + period.seconds() *
|
||||
hw_velocities_[i];
|
||||
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Got position state %.5f and velocity state %.5f for '%s'!",
|
||||
hw_positions_[i], hw_velocities_[i],
|
||||
info_.joints[i].name.c_str());
|
||||
}
|
||||
// END: This part here is for exemplary purposes - Please do not copy to
|
||||
your
|
||||
// production code
|
||||
*/
|
||||
|
||||
// Allocate memory for read buffer, set size according to your needs
|
||||
|
||||
// Read bytes. The behaviour of read() (e.g. does it block?,
|
||||
// how long does it block for?) depends on the configuration
|
||||
// settings above, specifically VMIN and VTIME
|
||||
// char read_buf[256];
|
||||
// int num_bytes = read(serial_port, &read_buf, sizeof(read_buf));
|
||||
|
||||
// // n is the number of bytes read. n may be 0 if no bytes were received,
|
||||
// and
|
||||
// // can also be -1 to signal an error.
|
||||
// if (num_bytes < 0) {
|
||||
// printf("Error reading: %s", strerror(errno));
|
||||
// return 1;
|
||||
// }
|
||||
|
||||
// // Here we assume we received ASCII data, but you might be sending raw
|
||||
// bytes
|
||||
// // (in that case, don't try and print it to the screen like this!)
|
||||
// printf("Read %i bytes. Received message: %s", num_bytes, read_buf);
|
||||
|
||||
// close(serial_port);
|
||||
if (!sync)
|
||||
return hardware_interface::return_type::ERROR;
|
||||
|
||||
uint8_t read_buf[1024];
|
||||
int read_buf_used =
|
||||
read_serial(serial_port_handle, read_buf, sizeof(read_buf));
|
||||
sync->handle_stream((uint8_t *)read_buf, read_buf_used);
|
||||
for (auto &[key, value] : interfaces_map) {
|
||||
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
// "Interface %s added (factor: %f).", key.c_str(),
|
||||
// value.factor);
|
||||
if (!value.command) {
|
||||
auto n = sync->get_number(key.c_str());
|
||||
if (n)
|
||||
value.value = value.factor * (*n);
|
||||
}
|
||||
}
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
if (std::chrono::steady_clock::now() - sync->last_pkg_recived >
|
||||
std::chrono::milliseconds(1000)) {
|
||||
RCLCPP_FATAL(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"last package recivew more than 1000ms ago. aborting!");
|
||||
return hardware_interface::return_type::ERROR;
|
||||
}
|
||||
|
||||
static int counter = 0;
|
||||
if (counter++ % 20 == 0)
|
||||
for (auto &i : sync->interfaces) {
|
||||
if (!i)
|
||||
continue;
|
||||
auto number_ptr = sync->get_number(i->key.c_str());
|
||||
auto str_ptr = sync->get_string(i->key.c_str());
|
||||
if (number_ptr)
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Sync: %s = %d", i->key.c_str(), *number_ptr);
|
||||
if (str_ptr)
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Sync: %s = %s", i->key.c_str(), str_ptr->c_str());
|
||||
}
|
||||
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
hardware_interface::return_type
|
||||
cyphal_hardware_interface ::cyphalSystemHardware::write(
|
||||
const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
|
||||
// BEGIN: This part here is for exemplary purposes - Please do not copy to
|
||||
// your production code
|
||||
|
||||
if (sync) {
|
||||
for (auto &[key, value] : interfaces_map) {
|
||||
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
// "Interface %s added (factor: %f).", key.c_str(),
|
||||
// value.factor);
|
||||
if (value.command) {
|
||||
auto ni = sync->get_or_create_interface(0x01, key.c_str());
|
||||
if (ni) {
|
||||
|
||||
*(int *)ni->data = (value.value / value.factor);
|
||||
ni->send_requested = true;
|
||||
|
||||
// RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
// "update: %s set to %d ", key.c_str(), *(int
|
||||
// *)ni->data);
|
||||
}
|
||||
}
|
||||
}
|
||||
sync->update();
|
||||
}
|
||||
|
||||
/* for (auto i = 0u; i < hw_commands_.size(); i++) {
|
||||
// Simulate sending commands to the hardware
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Got command %.5f for '%s'!", hw_commands_[i],
|
||||
info_.joints[i].name.c_str());
|
||||
|
||||
hw_velocities_[i] = hw_commands_[i];
|
||||
}
|
||||
RCLCPP_INFO(rclcpp::get_logger("cyphalSystemHardware"),
|
||||
"Joints successfully written!");
|
||||
// END: This part here is for exemplary purposes - Please do not copy to
|
||||
your
|
||||
// production code
|
||||
*/
|
||||
return hardware_interface::return_type::OK;
|
||||
}
|
||||
|
||||
} // namespace cyphal_hardware_interface
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(cyphal_hardware_interface::cyphalSystemHardware,
|
||||
hardware_interface::SystemInterface)
|
||||
40
package.xml
Normal file
40
package.xml
Normal file
@ -0,0 +1,40 @@
|
||||
<?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>cyphal_hardware_interface</name>
|
||||
<version>0.0.1</version>
|
||||
<description>`ros2_control` hardware interface for devices talking cyphal (udral_servo)</description>
|
||||
|
||||
<maintainer email="git@nilsschulte.de">Nils Schulte</maintainer>
|
||||
|
||||
<author email="git@nilsschulte.de">Nils Schulte</author>
|
||||
|
||||
<license>EUPLv1.2</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
|
||||
<exec_depend>controller_manager</exec_depend>
|
||||
<exec_depend>diff_drive_controller</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>ros2controlcli</exec_depend>
|
||||
<exec_depend>ros2launch</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
<test_depend>launch_testing_ament_cmake</test_depend>
|
||||
<test_depend>launch_testing_ros</test_depend>
|
||||
<test_depend>liburdfdom-tools</test_depend>
|
||||
<test_depend>xacro</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Loading…
x
Reference in New Issue
Block a user