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