From 0c69e889fb2c1235fc38789d77d837214b4945a0 Mon Sep 17 00:00:00 2001 From: Nils Schulte Date: Sat, 28 Jun 2025 21:38:25 +0200 Subject: [PATCH] init --- CMakeLists.txt | 96 ++++ README.md | 12 + bringup/config/staubi_controllers.yaml | 77 +++ bringup/config/staubi_nav2.yaml | 445 ++++++++++++++++++ bringup/launch/staubi.launch.py | 253 ++++++++++ cyphal_hardware_interface.xml | 9 + description/launch/view_robot.launch.py | 111 +++++ .../ros2_control/staubi.ros2_control.xacro | 63 +++ description/urdf/staubi.materials.xacro | 40 ++ description/urdf/staubi.urdf.xacro | 20 + .../urdf/staubi_description.urdf.xacro | 153 ++++++ .../simplesync_system.hpp | 104 ++++ .../visibility_control.h | 56 +++ hardware/simplesync_system.cpp | 430 +++++++++++++++++ package.xml | 40 ++ 15 files changed, 1909 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 bringup/config/staubi_controllers.yaml create mode 100644 bringup/config/staubi_nav2.yaml create mode 100644 bringup/launch/staubi.launch.py create mode 100644 cyphal_hardware_interface.xml create mode 100644 description/launch/view_robot.launch.py create mode 100644 description/ros2_control/staubi.ros2_control.xacro create mode 100644 description/urdf/staubi.materials.xacro create mode 100644 description/urdf/staubi.urdf.xacro create mode 100644 description/urdf/staubi_description.urdf.xacro create mode 100644 hardware/include/cyphal_hardware_interface/simplesync_system.hpp create mode 100644 hardware/include/cyphal_hardware_interface/visibility_control.h create mode 100644 hardware/simplesync_system.cpp create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..848131a --- /dev/null +++ b/CMakeLists.txt @@ -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 +$ +$ +${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() diff --git a/README.md b/README.md new file mode 100644 index 0000000..3449ef7 --- /dev/null +++ b/README.md @@ -0,0 +1,12 @@ +# cyphal_hardware_interface + + + + + + +# 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" +``` + diff --git a/bringup/config/staubi_controllers.yaml b/bringup/config/staubi_controllers.yaml new file mode 100644 index 0000000..3415db2 --- /dev/null +++ b/bringup/config/staubi_controllers.yaml @@ -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 diff --git a/bringup/config/staubi_nav2.yaml b/bringup/config/staubi_nav2.yaml new file mode 100644 index 0000000..bc14d7e --- /dev/null +++ b/bringup/config/staubi_nav2.yaml @@ -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 diff --git a/bringup/launch/staubi.launch.py b/bringup/launch/staubi.launch.py new file mode 100644 index 0000000..1f39456 --- /dev/null +++ b/bringup/launch/staubi.launch.py @@ -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) diff --git a/cyphal_hardware_interface.xml b/cyphal_hardware_interface.xml new file mode 100644 index 0000000..fff6e62 --- /dev/null +++ b/cyphal_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + The ros2_control cyphal_hardware_interface using a system hardware interface-type. + + + diff --git a/description/launch/view_robot.launch.py b/description/launch/view_robot.launch.py new file mode 100644 index 0000000..a5b81ed --- /dev/null +++ b/description/launch/view_robot.launch.py @@ -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) diff --git a/description/ros2_control/staubi.ros2_control.xacro b/description/ros2_control/staubi.ros2_control.xacro new file mode 100644 index 0000000..4b09df6 --- /dev/null +++ b/description/ros2_control/staubi.ros2_control.xacro @@ -0,0 +1,63 @@ + + + + + + + + + cyphal_hardware_interface/cyphalSystemHardware + /dev/ttyAMA10 + + 460800 + + + + + + mock_components/GenericSystem + true + + + + vcc + 0.001 + + + + s + 0.1427 + + 0 + 1 + + + + vt0 + a0 + v0 + 0.0001 + + + + + + + + vt1 + a1 + v1 + 0.0001 + + + + + + + + + + + diff --git a/description/urdf/staubi.materials.xacro b/description/urdf/staubi.materials.xacro new file mode 100644 index 0000000..035bf58 --- /dev/null +++ b/description/urdf/staubi.materials.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/description/urdf/staubi.urdf.xacro b/description/urdf/staubi.urdf.xacro new file mode 100644 index 0000000..fbc88c9 --- /dev/null +++ b/description/urdf/staubi.urdf.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + diff --git a/description/urdf/staubi_description.urdf.xacro b/description/urdf/staubi_description.urdf.xacro new file mode 100644 index 0000000..ffc3f7d --- /dev/null +++ b/description/urdf/staubi_description.urdf.xacro @@ -0,0 +1,153 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hardware/include/cyphal_hardware_interface/simplesync_system.hpp b/hardware/include/cyphal_hardware_interface/simplesync_system.hpp new file mode 100644 index 0000000..4af08f0 --- /dev/null +++ b/hardware/include/cyphal_hardware_interface/simplesync_system.hpp @@ -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 +#include +#include +#include + +#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 + export_state_interfaces() override; + + CYPHAL_HARDWARE_INTERFACE_PUBLIC + std::vector + 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 interfaces_map; + typedef simplesync::cyphal< + std::chrono::time_point, + std::chrono::nanoseconds, 1024, true> + cyphalChrono; + std::unique_ptr sync; +}; + +} // namespace cyphal_hardware_interface + +#endif // CYPHAL_HARDWARE_INTERFACE__SIMPLESYNC_SYSTEM_HPP_ diff --git a/hardware/include/cyphal_hardware_interface/visibility_control.h b/hardware/include/cyphal_hardware_interface/visibility_control.h new file mode 100644 index 0000000..9dd670a --- /dev/null +++ b/hardware/include/cyphal_hardware_interface/visibility_control.h @@ -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_ diff --git a/hardware/simplesync_system.cpp b/hardware/simplesync_system.cpp new file mode 100644 index 0000000..3e7977f --- /dev/null +++ b/hardware/simplesync_system.cpp @@ -0,0 +1,430 @@ +#include "cyphal_hardware_interface/simplesync_system.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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({"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 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::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::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 +cyphalSystemHardware::export_state_interfaces() { + std::vector 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 +cyphalSystemHardware::export_command_interfaces() { + std::vector 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( + [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) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..d425ce3 --- /dev/null +++ b/package.xml @@ -0,0 +1,40 @@ + + + + cyphal_hardware_interface + 0.0.1 + `ros2_control` hardware interface for devices talking cyphal (udral_servo) + + Nils Schulte + + Nils Schulte + + EUPLv1.2 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + + controller_manager + diff_drive_controller + joint_state_broadcaster + robot_state_publisher + ros2controlcli + ros2launch + rviz2 + xacro + + ament_cmake_gtest + ament_cmake_pytest + launch_testing_ament_cmake + launch_testing_ros + liburdfdom-tools + xacro + + + ament_cmake + +