diff --git a/.gitignore b/.gitignore index 979cf4c2a..e2b42f231 100644 --- a/.gitignore +++ b/.gitignore @@ -59,3 +59,6 @@ build-*/ *.txt.user *.gch /.project + +# IDE specific files +.vscode/ diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 000000000..bd554e6a5 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,23 @@ +# TurtleBot3 ROS 2 Codebase Guide + +## Build Commands +- **Build single package**: `colcon build --packages-select turtlebot3_node` +- **Build all**: `colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release` +- **Source workspace**: `source install/setup.bash` +- **Launch robot**: Launching the robot is only possible from the turtlebot's raspberry pi. You, the agentic coding tool, only operate on the codebase from the remote PC. Therefore launching is not something you can do. +- **View analog data**: `ros2 topic echo /analog_pins` + +## Architecture +- **Main packages**: `turtlebot3_node` (core driver), `turtlebot3_bringup` (launch files), `turtlebot3_description` (URDF), `turtlebot3_navigation2`, `turtlebot3_teleop` +- **Core class**: `robotis::turtlebot3::TurtleBot3` - main node managing sensors, devices, motors +- **New feature**: Analog pins A0-A5 published to `/analog_pins` topic via `AnalogPins` sensor class +- **Communication**: Uses DynamixelSDKWrapper to read from OpenCR control table addresses 350-360 for analog pins +- **Dependencies**: ROS 2 Humble, Dynamixel SDK, custom OpenCR firmware with analog support + +## Code Style +- **Standard**: C++17, follows ROS 2 conventions +- **Naming**: snake_case for variables/functions, PascalCase for classes, namespaces: `robotis::turtlebot3::` +- **Includes**: System includes first, then ROS msgs, then local headers with relative paths +- **Error handling**: try-catch blocks with RCLCPP_ERROR logging +- **Memory**: Smart pointers (std::shared_ptr, std::unique_ptr), RAII patterns +- **Formatting**: 2-space indentation, header guards with full path, Apache 2.0 license headers diff --git a/README.md b/README.md index 011fb0f3b..d9d074ef3 100644 --- a/README.md +++ b/README.md @@ -1,41 +1,36 @@ -# TurtleBot3 - +# Overview +This is a fork of [ROBOTIS-GIT/turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3). This `publish-analog-pins` branch extends the `turtlebot3_node` to broadcast data collected by the OpenCR analog pins A0-A5 to a ROS 2 topic `/analog_pins`. The `main` branch is kept in sync with the ROBOTIS repository. -- Active Branches: noetic, humble, jazzy, main(rolling) -- Legacy Branches: *-devel +This repo is part of the larger project [ez-turtlebot3](https://github.com/ez-turtlebot3/ez-turtlebot3), which combines this repo with analog-enabled [OpenCR](https://github.com/ez-turtlebot3/OpenCR) firmware and a [ROS 2 analog processor package](https://github.com/ez-turtlebot3/ez_analog_processor/branches) to read, process, and publish analog data while operating a TurtleBot3. + +# Requirements +* [ROS 2 Humble](https://docs.ros.org/en/humble/index.html) +* [Analog-enabled TurtleBot3 ROS 2 OpenCR firmware](https://github.com/ez-turtlebot3/OpenCR) -## Open Source Projects Related to TurtleBot3 -- [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) -- [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) -- [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) -- [turtlebot3_manipulation](https://github.com/ROBOTIS-GIT/turtlebot3_manipulation) -- [turtlebot3_manipulation_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_manipulation_simulations) -- [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) -- [turtlebot3_applications_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_applications_msgs) -- [turtlebot3_machine_learning](https://github.com/ROBOTIS-GIT/turtlebot3_machine_learning) -- [turtlebot3_autorace](https://github.com/ROBOTIS-GIT/turtlebot3_autorace) -- [turtlebot3_home_service_challenge](https://github.com/ROBOTIS-GIT/turtlebot3_home_service_challenge) -- [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver) -- [ld08_driver](https://github.com/ROBOTIS-GIT/ld08_driver) -- [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) -- [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) -- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) -- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) +# Installation +1. If you haven't already, follow the Humble instructions for the [TurtleBot3 SBC setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/sbc_setup/#sbc-setup) +2. Replace the turtlebot3 repo with the analog-enabled fork + * `cd ~/turtlebot3_ws/src` + * `rm -rf turtlebot3` + * `git clone https://github.com/ez-turtlebot3/turtlebot3` +3. Rebuild the turtlebot3_node + * `cd ~/turtlebot3_ws` + * `colcon build --packages-select turtlebot3_node` -## Documentation, Videos, and Community +# Use +1. Launch the TurtleBot with the same bringup command as before + * `ros2 launch turtlebot3_bringup robot.launch.py` +2. View the /analog_pins topic from your remote pc with + * `ros2 topic echo /analog_pins` +3. Optionally, install the [ROS 2 analog processor package](https://github.com/ez-turtlebot3/ez_analog_processor/branches) to process the raw data from `/analog_pins`. +4. Recommendation: view real-time data in plots with [PlotJuggler](https://github.com/facontidavide/PlotJuggler) -### Official Documentation +# ROBOTIS links +## Official Documentation - ⚙️ **[ROBOTIS DYNAMIXEL](https://dynamixel.com/)** - 📚 **[ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/)** - 📚 **[ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/)** -- 📚 **[ROBOTIS e-Manual for OpenMANIPULATOR-X](https://emanual.robotis.com/docs/en/platform/openmanipulator_x/overview/)** -### Learning Resources -- 🎥 **[ROBOTIS YouTube Channel](https://www.youtube.com/@ROBOTISCHANNEL)** -- 🎥 **[ROBOTIS Open Source YouTube Channel](https://www.youtube.com/@ROBOTISOpenSourceTeam)** -- 🎥 **[ROBOTIS TurtleBot3 YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU)** -- 🎥 **[ROBOTIS OpenMANIPULATOR YouTube Playlist](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb)** - -### Community & Support +## Community & Support - 💬 **[ROBOTIS Community Forum](https://forum.robotis.com/)** - 💬 **[TurtleBot category from ROS Community](https://discourse.ros.org/c/turtlebot/)** diff --git a/turtlebot3_bringup/launch/robot.launch.py b/turtlebot3_bringup/launch/robot.launch.py index fbb819a06..f819cdf72 100644 --- a/turtlebot3_bringup/launch/robot.launch.py +++ b/turtlebot3_bringup/launch/robot.launch.py @@ -96,7 +96,10 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource( - [ThisLaunchFileDir(), '/turtlebot3_state_publisher.launch.py']), + [os.path.join( + get_package_share_directory('turtlebot3_bringup'), + 'launch', + 'turtlebot3_state_publisher.launch.py')]), launch_arguments={'use_sim_time': use_sim_time, 'namespace': namespace}.items(), ), diff --git a/turtlebot3_bringup/param/humble/burger.yaml b/turtlebot3_bringup/param/humble/burger.yaml index b32b3178d..c01c9462d 100644 --- a/turtlebot3_bringup/param/humble/burger.yaml +++ b/turtlebot3_bringup/param/humble/burger.yaml @@ -26,6 +26,9 @@ illumination: 0 ir: 0 sonar: 0 + # Analog pins to read from (0-5). Must match firmware CONNECTED_ANALOG_PINS configuration + analog_pins: [2, 3, 4] + /**: diff_drive_controller: ros__parameters: diff --git a/turtlebot3_navigation2/param/burger.yaml b/turtlebot3_navigation2/param/burger.yaml index 711d09584..759f57d4f 100644 --- a/turtlebot3_navigation2/param/burger.yaml +++ b/turtlebot3_navigation2/param/burger.yaml @@ -1,5 +1,6 @@ amcl: ros__parameters: + use_sim_time: False alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -29,7 +30,7 @@ amcl: save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true - transform_tolerance: 0.5 + transform_tolerance: 1.0 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 @@ -37,128 +38,173 @@ amcl: z_rand: 0.5 z_short: 0.05 scan_topic: scan - map_topic: map - set_initial_pose: false - always_reset_initial_pose: false - first_map_only: false - initial_pose: - x: 0.0 - y: 0.0 - z: 0.0 - yaw: 0.0 + +amcl_map_client: + ros__parameters: + use_sim_time: False + +amcl_rclcpp_node: + ros__parameters: + use_sim_time: False bt_navigator: ros__parameters: + use_sim_time: False global_frame: map robot_base_frame: base_link - transform_tolerance: 0.5 - filter_duration: 0.3 - default_nav_to_pose_bt_xml: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" # behavior trees location. - default_nav_through_poses_bt_xml: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" - always_reload_bt_xml: false - goal_blackboard_id: goal - goals_blackboard_id: goals - path_blackboard_id: path - navigators: ['navigate_to_pose', 'navigate_through_poses'] - navigate_to_pose: - plugin: "nav2_bt_navigator::NavigateToPoseNavigator" - navigate_through_poses: - plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - error_code_name_prefixes: - - assisted_teleop - - backup - - compute_path - - dock_robot - - drive_on_heading - - follow_path - - nav_thru_poses - - nav_to_pose - - spin - - route - - undock_robot - - wait + odom_topic: /odom + default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml" + bt_loop_duration: 10 + default_server_timeout: 20 + enable_groot_monitoring: True + groot_zmq_publisher_port: 1666 + groot_zmq_server_port: 1667 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_back_up_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node -docking_server: +bt_navigator_rclcpp_node: ros__parameters: - dock_plugins: ['nova_carter_dock'] - nova_carter_dock: - plugin: 'opennav_docking::SimpleChargingDock' - docks: ['home_dock','flex_dock1', 'flex_dock2'] - home_dock: - type: 'nova_carter_dock' - frame: map - pose: [0.0, 0.0, 0.0] - flex_dock1: - type: 'nova_carter_dock' - frame: map - pose: [10.0, 10.0, 0.0] - flex_dock2: - type: 'nova_carter_dock' - frame: map - pose: [30.0, 30.0, 0.0] - enable_stamped_cmd_vel: true + use_sim_time: False controller_server: ros__parameters: - controller_frequency: 10.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.001 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugins: ["progress_checker"] - goal_checker_plugins: ["goal_checker"] - controller_plugins: ["FollowPath"] - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.1 - movement_time_allowance: 10.0 - goal_checker: - stateful: true - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 + controller_frequency: 20.0 FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: true - min_vel_x: 0.0 - min_vel_y: 0.0 - max_vel_x: 0.3 - max_vel_y: 0.0 - max_vel_theta: 1.0 - min_speed_xy: 0.0 - max_speed_xy: 0.3 - min_speed_theta: 0.0 - # Add high threshold velocity for turtlebot 3 issue. - # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 - acc_lim_x: 3.0 - 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: 0 - vtheta_samples: 40 - sim_time: 1.5 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.05 - 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 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - 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 - enable_stamped_cmd_vel: true + 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.2 # OOB: 0.5 + vx_min: -0.35 + vy_max: 0.2 # OOB: 0.5 + wz_max: 1.9 + ax_max: 3.0 + ax_min: -3.0 + ay_max: 3.0 + az_max: 3.5 + iteration_count: 1 + prune_distance: 1.7 + transform_tolerance: 0.1 + temperature: 0.3 + gamma: 0.015 + motion_model: "DiffDrive" + visualize: false + reset_period: 1.0 # (only in Humble) + regenerate_noises: false + 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 + # ObstaclesCritic: + # enabled: true + # cost_power: 1 + # repulsion_weight: 1.5 + # critical_weight: 20.0 + # consider_footprint: false + # collision_cost: 10000.0 + # collision_margin_distance: 0.1 + # near_goal_distance: 0.5 + # inflation_radius: 0.55 # (only in Humble) + # cost_scaling_factor: 10.0 # (only in Humble) + 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: true + 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 + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] + # TwirlingCritic: + # enabled: true + # twirling_cost_power: 1 + # twirling_cost_weight: 10.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: False local_costmap: local_costmap: @@ -167,30 +213,31 @@ local_costmap: publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link + use_sim_time: False rolling_window: true width: 3 height: 3 resolution: 0.05 - robot_radius: 0.1 + robot_radius: 0.105 plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - inflation_radius: 0.5 - cost_scaling_factor: 5.0 + inflation_radius: 1.0 + cost_scaling_factor: 3.0 obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: true + enabled: True observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" - enabled: true - publish_voxel_map: true + enabled: True + publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 @@ -200,16 +247,22 @@ local_costmap: scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + 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: - map_subscribe_transient_local: true - always_send_full_costmap: true + map_subscribe_transient_local: True + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: False + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False global_costmap: global_costmap: @@ -218,20 +271,20 @@ global_costmap: publish_frequency: 1.0 global_frame: map robot_base_frame: base_link - transform_tolerance: 0.5 - robot_radius: 0.1 + use_sim_time: True + robot_radius: 0.105 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: true + enabled: True observation_sources: scan scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 @@ -239,8 +292,8 @@ global_costmap: obstacle_min_range: 0.0 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" - enabled: true - publish_voxel_map: true + enabled: True + publish_voxel_map: True origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 @@ -250,8 +303,8 @@ global_costmap: scan: topic: /scan max_obstacle_height: 2.0 - clearing: true - marking: true + clearing: True + marking: True data_type: "LaserScan" raytrace_max_range: 3.0 raytrace_min_range: 0.0 @@ -259,175 +312,133 @@ global_costmap: obstacle_min_range: 0.0 static_layer: plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: true - transform_tolerance: 0.1 + map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - inflation_radius: 0.5 - cost_scaling_factor: 5.0 - always_send_full_costmap: true + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: False + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: False map_server: ros__parameters: + use_sim_time: False yaml_filename: "map.yaml" map_saver: ros__parameters: + use_sim_time: False save_map_timeout: 5.0 free_thresh_default: 0.25 occupied_thresh_default: 0.65 - map_subscribe_transient_local: true + map_subscribe_transient_local: True planner_server: ros__parameters: - expected_planner_frequency: 10.0 planner_plugins: ["GridBased"] + use_sim_time: True + GridBased: - plugin: "nav2_navfn_planner::NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true + plugin: "nav2_smac_planner/SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "REEDS_SHEPP" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.2 # minimum turning radius in m of path / vehicle + reverse_penalty: 1.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path -behavior_server: + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: False + +recoveries_server: ros__parameters: - local_costmap_topic: local_costmap/costmap_raw - local_footprint_topic: local_costmap/published_footprint - global_costmap_topic: global_costmap/costmap_raw - global_footprint_topic: global_costmap/published_footprint + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "wait", "assisted_teleop"] - spin: - plugin: "nav2_behaviors::Spin" # In Iron and older versions, "/" was used instead of "::" - backup: - plugin: "nav2_behaviors::BackUp" # In Iron and older versions, "/" was used instead of "::" - drive_on_heading: - plugin: "nav2_behaviors::DriveOnHeading" # In Iron and older versions, "/" was used instead of "::" - wait: - plugin: "nav2_behaviors::Wait" # In Iron and older versions, "/" was used instead of "::" - assisted_teleop: - plugin: "nav2_behaviors::AssistedTeleop" # In Iron and older versions, "/" was used instead of "::" - local_frame: odom - global_frame: map + recovery_plugins: ["wait", "backup", "rotate1", "rotate2", "rotate3"] # Reordered and added multiple rotates + global_frame: odom robot_base_frame: base_link transform_timeout: 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 + use_sim_time: false + # simulate_ahead_time: 2.0 # This parameter seems to be misplaced, it belongs to backup/spin, but backup/spin do not use simulate_ahead_time + # # max_rotational_vel: 1.0 # Not needed here, as set in each spin config + # # min_rotational_vel: 0.4 + # # rotational_acc_lim: 3.2 + + wait: + plugin: "nav2_recoveries/Wait" + ros__parameters: + wait_duration: 5.0 # Wait for 5 seconds + + backup: + plugin: "nav2_recoveries/BackUp" + # ros__parameters: + # backup_dist: 0.15 # Backup distance in meters (adjust as needed) + # backup_speed: 0.1 # Backup speed in m/s (adjust as needed) + # #simulate_ahead_time: 2.0 #Not used in BackUp plugin + + rotate1: # First 90-degree rotation + plugin: "nav2_recoveries/Spin" + ros__parameters: + yaw_rot_vel: 1.0 # Desired rotational speed + target_yaw: 1.57 # Target angle (90 degrees in radians) + + rotate2: # Second 90-degree rotation + plugin: "nav2_recoveries/Spin" + ros__parameters: + yaw_rot_vel: 1.0 + target_yaw: 3.14 # Target angle (180 degrees in radians) + + rotate3: # Third 90 degree rotation + plugin: "nav2_recoveries/Spin" + ros__parameters: + yaw_rot_vel: 1.0 + target_yaw: 4.71 # Target angle (270 degrees in radians) + +robot_state_publisher: + ros__parameters: + use_sim_time: False waypoint_follower: ros__parameters: - loop_rate: 20 + loop_rate: 2000 stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" + waypoint_task_executor_plugin: "wait_at_waypoint" wait_at_waypoint: plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: true - waypoint_pause_duration: 200 - -collision_monitor: - ros__parameters: - base_frame_id: "base_footprint" - 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.5 - source_timeout: 5.0 - base_shift_correction: true - stop_pub_timeout: 2.0 - enable_stamped_cmd_vel: true - use_realtime_priority: false - polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"] - PolygonStop: - type: "circle" - radius: 0.1 - action_type: "stop" - min_points: 4 - visualize: true - polygon_pub_topic: "polygon_stop" - enabled: true - PolygonSlow: - type: "polygon" - points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]" - action_type: "slowdown" - min_points: 4 - slowdown_ratio: 0.3 - visualize: true - polygon_pub_topic: "polygon_slowdown" - enabled: true - PolygonLimit: - type: "polygon" - points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]" - action_type: "limit" - min_points: 4 - linear_limit: 0.4 - angular_limit: 0.5 - visualize: true - polygon_pub_topic: "polygon_limit" - enabled: true - FootprintApproach: - type: "polygon" - action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" - time_before_collision: 2.0 - simulation_time_step: 0.02 - min_points: 6 - visualize: False - enabled: true - VelocityPolygonStop: - type: "velocity_polygon" - action_type: "stop" - min_points: 6 - visualize: true - enabled: False - polygon_pub_topic: "velocity_polygon_stop" - velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"] - holonomic: false - rotation: - points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]" - linear_min: 0.0 - linear_max: 0.05 - theta_min: -1.0 - theta_max: 1.0 - translation_forward: - points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]" - linear_min: 0.0 - linear_max: 1.0 - theta_min: -1.0 - theta_max: 1.0 - translation_backward: - points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]" - linear_min: -1.0 - linear_max: 0.0 - theta_min: -1.0 - theta_max: 1.0 - stopped: - points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]" - linear_min: -1.0 - linear_max: 1.0 - theta_min: -1.0 - theta_max: 1.0 - observation_sources: ["scan"] - scan: - source_timeout: 0.2 - type: "scan" - topic: "/scan" - enabled: true - -velocity_smoother: - ros__parameters: - smoothing_frequency: 20.0 - scale_velocities: false - feedback: "OPEN_LOOP" - max_velocity: [0.5, 0.0, 2.5] - min_velocity: [-0.5, 0.0, -2.5] - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 - max_accel: [2.5, 0.0, 3.2] - max_decel: [-2.5, 0.0, -3.2] - odom_topic: "odom" - odom_duration: 0.1 - use_realtime_priority: false - enable_stamped_cmd_vel: true + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/turtlebot3_node/CMakeLists.txt b/turtlebot3_node/CMakeLists.txt index d81d81276..555d465f0 100644 --- a/turtlebot3_node/CMakeLists.txt +++ b/turtlebot3_node/CMakeLists.txt @@ -17,6 +17,7 @@ endif() ################################################################################ find_package(ament_cmake REQUIRED) find_package(dynamixel_sdk REQUIRED) +find_package(ez_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) @@ -49,11 +50,14 @@ add_library(${PROJECT_NAME}_lib "src/sensors/battery_state.cpp" "src/sensors/imu.cpp" "src/sensors/joint_state.cpp" - "src/sensors/sensor_state.cpp" + "src/sensors/analog_pins.cpp" + "src/sensors/temperature.cpp" + "src/sensors/humidity.cpp" ) set(DEPENDENCIES "dynamixel_sdk" + "ez_interfaces" "geometry_msgs" "message_filters" "nav_msgs" diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 35e7e42c9..aa8a72913 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -46,15 +46,15 @@ typedef struct ControlItem baud_rate = {8, EEPROM, 1, READ}; ControlItem millis = {10, RAM, 4, READ}; - ControlItem micros = {14, RAM, 4, READ}; + // ControlItem micros = {14, RAM, 4, READ}; // Does not match firmware ControlItem device_status = {18, RAM, 1, READ}; ControlItem heartbeat = {19, RAM, 1, READ_WRITE}; - ControlItem external_led_1 = {20, RAM, 1, READ_WRITE}; - ControlItem external_led_2 = {21, RAM, 1, READ_WRITE}; - ControlItem external_led_3 = {22, RAM, 1, READ_WRITE}; - ControlItem external_led_4 = {23, RAM, 1, READ_WRITE}; + ControlItem external_led_1 = {20, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_1 + ControlItem external_led_2 = {21, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_2 + ControlItem external_led_3 = {22, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_3 + ControlItem external_led_4 = {23, RAM, 1, READ_WRITE}; // FW: ADDR_USER_LED_4 ControlItem button_1 = {26, RAM, 1, READ}; ControlItem button_2 = {27, RAM, 1, READ}; @@ -62,9 +62,12 @@ typedef struct ControlItem bumper_1 = {28, RAM, 1, READ}; ControlItem bumper_2 = {29, RAM, 1, READ}; - ControlItem illumination = {30, RAM, 4, READ}; - ControlItem ir = {34, RAM, 4, READ}; - ControlItem sonar = {38, RAM, 4, READ}; + ControlItem analog_a0 = {30, RAM, 2, READ}; + ControlItem analog_a1 = {32, RAM, 2, READ}; + ControlItem analog_a2 = {34, RAM, 2, READ}; + ControlItem analog_a3 = {36, RAM, 2, READ}; + ControlItem analog_a4 = {38, RAM, 2, READ}; + ControlItem analog_a5 = {40, RAM, 2, READ}; ControlItem battery_voltage = {42, RAM, 4, READ}; ControlItem battery_percentage = {46, RAM, 4, READ}; @@ -87,6 +90,9 @@ typedef struct ControlItem imu_orientation_y = {104, RAM, 4, READ}; ControlItem imu_orientation_z = {108, RAM, 4, READ}; + ControlItem temperature = {112, RAM, 4, READ}; + ControlItem humidity = {116, RAM, 4, READ}; + ControlItem present_current_left = {120, RAM, 4, READ}; ControlItem present_current_right = {124, RAM, 4, READ}; ControlItem present_velocity_left = {128, RAM, 4, READ}; @@ -94,7 +100,8 @@ typedef struct ControlItem present_position_left = {136, RAM, 4, READ}; ControlItem present_position_right = {140, RAM, 4, READ}; - ControlItem motor_torque_enable = {149, RAM, 1, READ_WRITE}; + // Missing: ADDR_MOTOR_CONNECT = 148 (FW has this but control table doesn't) + ControlItem motor_torque_enable = {149, RAM, 1, READ_WRITE}; // FW: ADDR_MOTOR_TORQUE ControlItem cmd_velocity_linear_x = {150, RAM, 4, READ_WRITE}; ControlItem cmd_velocity_linear_y = {154, RAM, 4, READ_WRITE}; @@ -105,6 +112,7 @@ typedef struct ControlItem profile_acceleration_left = {174, RAM, 4, READ_WRITE}; ControlItem profile_acceleration_right = {178, RAM, 4, READ_WRITE}; + } ControlTable; const ControlTable extern_control_table; diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp new file mode 100644 index 000000000..d447f1653 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/analog_pins.hpp @@ -0,0 +1,52 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// 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 TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_ +#define TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "ez_interfaces/msg/analog_pins.hpp" + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class AnalogPins : public Sensors +{ +public: + explicit AnalogPins( + std::shared_ptr & nh, + const std::string & topic_name); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + +private: + rclcpp::Publisher::SharedPtr analog_publisher_; + std::vector configured_pins_; // Which pins to read from +}; +} // namespace sensors +} // namespace turtlebot3 +} // namespace robotis + +#endif // TURTLEBOT3_NODE__SENSORS__ANALOG_PINS_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp new file mode 100644 index 000000000..e879b9fbb --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/humidity.hpp @@ -0,0 +1,39 @@ +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza + +#ifndef TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ +#define TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/relative_humidity.hpp" + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class Humidity : public Sensors +{ +public: + explicit Humidity( + std::shared_ptr & nh, + const std::string & topic_name); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + +private: + rclcpp::Publisher::SharedPtr humidity_publisher_; +}; +} // namespace sensors +} // namespace turtlebot3 +} // namespace robotis + +#endif // TURTLEBOT3_NODE__SENSORS__HUMIDITY_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp new file mode 100644 index 000000000..2cfdce799 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/temperature.hpp @@ -0,0 +1,39 @@ +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza + +#ifndef TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ +#define TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/temperature.hpp" + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class Temperature : public Sensors +{ +public: + explicit Temperature( + std::shared_ptr & nh, + const std::string & topic_name); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + +private: + rclcpp::Publisher::SharedPtr temperature_publisher_; +}; +} // namespace sensors +} // namespace turtlebot3 +} // namespace robotis + +#endif // TURTLEBOT3_NODE__SENSORS__TEMPERATURE_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp index fdc70e472..9c71d663a 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -50,6 +50,10 @@ #include "turtlebot3_node/sensors/sensors.hpp" #include "turtlebot3_node/twist_subscriber.hpp" +#include "turtlebot3_node/sensors/analog_pins.hpp" +#include "turtlebot3_node/sensors/temperature.hpp" +#include "turtlebot3_node/sensors/humidity.hpp" + namespace robotis { namespace turtlebot3 @@ -89,6 +93,7 @@ class TurtleBot3 : public rclcpp::Node void publish_timer(const std::chrono::milliseconds timeout); void heartbeat_timer(const std::chrono::milliseconds timeout); + void analog_pins_timer(const std::chrono::milliseconds timeout); void cmd_vel_callback(); void parameter_event_callback(); @@ -107,11 +112,14 @@ class TurtleBot3 : public rclcpp::Node rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::TimerBase::SharedPtr analog_pins_timer_; std::unique_ptr cmd_vel_sub_; rclcpp::AsyncParametersClient::SharedPtr priv_parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; + + sensors::AnalogPins * analog_pins_sensor_; }; } // namespace turtlebot3 } // namespace robotis diff --git a/turtlebot3_node/package.xml b/turtlebot3_node/package.xml index 6e4b9a512..3258a0070 100644 --- a/turtlebot3_node/package.xml +++ b/turtlebot3_node/package.xml @@ -17,6 +17,7 @@ Hyungyu Kim ament_cmake dynamixel_sdk + ez_interfaces geometry_msgs message_filters nav_msgs diff --git a/turtlebot3_node/param/burger.yaml b/turtlebot3_node/param/burger.yaml index 5263900a1..d7f6bd891 100644 --- a/turtlebot3_node/param/burger.yaml +++ b/turtlebot3_node/param/burger.yaml @@ -23,6 +23,9 @@ turtlebot3_node: illumination: 0 ir: 0 sonar: 0 + + # Analog pins to read from (0-5). Must match firmware CONNECTED_ANALOG_PINS configuration + analog_pins: [0, 1, 2, 3, 4, 5] diff_drive_controller: ros__parameters: diff --git a/turtlebot3_node/src/sensors/analog_pins.cpp b/turtlebot3_node/src/sensors/analog_pins.cpp new file mode 100644 index 000000000..ec25ed307 --- /dev/null +++ b/turtlebot3_node/src/sensors/analog_pins.cpp @@ -0,0 +1,107 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// 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. + +#include "turtlebot3_node/sensors/analog_pins.hpp" + +#include +#include +#include + +#include "turtlebot3_node/control_table.hpp" +#include "ez_interfaces/msg/analog_pins.hpp" + +using robotis::turtlebot3::sensors::AnalogPins; + +AnalogPins::AnalogPins( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh, "") // Call parent constructor with node handle and empty frame_id +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + analog_publisher_ = nh->create_publisher(topic_name, qos); + + // Read analog pin configuration from parameters + nh->declare_parameter>("sensors.analog_pins", std::vector{0, 1, 2, 3, 4, 5}); + configured_pins_ = nh->get_parameter("sensors.analog_pins").as_integer_array(); + + // Validate pin numbers (must be 0-5) + for (auto pin : configured_pins_) { + if (pin < 0 || pin > 5) { + RCLCPP_WARN(nh->get_logger(), "Invalid analog pin %ld, must be 0-5", pin); + } + } + + RCLCPP_INFO(nh->get_logger(), "Analog pins configured for pins: [%s]", + [this]() { + std::string pins_str; + for (size_t i = 0; i < configured_pins_.size(); ++i) { + pins_str += std::to_string(configured_pins_[i]); + if (i < configured_pins_.size() - 1) pins_str += ", "; + } + return pins_str; + }().c_str()); + + RCLCPP_INFO(nh->get_logger(), "Succeeded to create analog pins publisher"); +} + +void AnalogPins::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + try { + auto analog_msg = std::make_unique(); + + // Set timestamp + analog_msg->header.stamp = now; + analog_msg->header.frame_id = "base_link"; + + // Array of control table entries for easy access + const robotis::turtlebot3::ControlItem* analog_entries[] = { + &extern_control_table.analog_a0, + &extern_control_table.analog_a1, + &extern_control_table.analog_a2, + &extern_control_table.analog_a3, + &extern_control_table.analog_a4, + &extern_control_table.analog_a5 + }; + + // Create AnalogPin message for each configured pin + analog_msg->pins.resize(configured_pins_.size()); + + for (size_t i = 0; i < configured_pins_.size(); ++i) { + int pin = static_cast(configured_pins_[i]); + if (pin >= 0 && pin <= 5) { + analog_msg->pins[i].pin = static_cast(pin); + analog_msg->pins[i].value = dxl_sdk_wrapper->get_data_from_device( + analog_entries[pin]->addr, + analog_entries[pin]->length); + } else { + analog_msg->pins[i].pin = static_cast(pin); + analog_msg->pins[i].value = 0; // Invalid pin, set to 0 + } + } + + // Publish the message + analog_publisher_->publish(std::move(analog_msg)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("analog_pins"), + "Exception in analog_pins publish: %s", e.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("analog_pins"), + "Unknown exception in analog_pins publish"); + } +} diff --git a/turtlebot3_node/src/sensors/humidity.cpp b/turtlebot3_node/src/sensors/humidity.cpp new file mode 100644 index 000000000..93e34545a --- /dev/null +++ b/turtlebot3_node/src/sensors/humidity.cpp @@ -0,0 +1,61 @@ +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza + +#include "turtlebot3_node/sensors/humidity.hpp" + +#include +#include +#include + +#include "turtlebot3_node/control_table.hpp" +#include "sensor_msgs/msg/relative_humidity.hpp" + +using robotis::turtlebot3::sensors::Humidity; + +Humidity::Humidity( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh, "base_link") +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + humidity_publisher_ = nh->create_publisher(topic_name, qos); + + RCLCPP_INFO(nh->get_logger(), "Succeeded to create humidity publisher"); +} + +void Humidity::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + try { + auto humidity_msg = std::make_unique(); + + // Set timestamp and frame + humidity_msg->header.stamp = now; + humidity_msg->header.frame_id = frame_id_; + + // Read humidity from control table (address 116, 4 bytes) + // Firmware stores as float from DHT sensor (percentage) + float humidity_percent = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.humidity.addr, + extern_control_table.humidity.length); + + // Convert percentage to ratio (0.0-1.0) for ROS standard compliance + humidity_msg->relative_humidity = humidity_percent / 100.0; + + // Set variance to 0 (unknown) + humidity_msg->variance = 0.0; + + // Publish the message + humidity_publisher_->publish(std::move(humidity_msg)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("humidity"), + "Exception in humidity publish: %s", e.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("humidity"), + "Unknown exception in humidity publish"); + } +} diff --git a/turtlebot3_node/src/sensors/temperature.cpp b/turtlebot3_node/src/sensors/temperature.cpp new file mode 100644 index 000000000..606ac5ced --- /dev/null +++ b/turtlebot3_node/src/sensors/temperature.cpp @@ -0,0 +1,58 @@ +// Copyright 2025 EyeZense, Inc. +// Author: Travis Mendoza + +#include "turtlebot3_node/sensors/temperature.hpp" + +#include +#include +#include + +#include "turtlebot3_node/control_table.hpp" +#include "sensor_msgs/msg/temperature.hpp" + +using robotis::turtlebot3::sensors::Temperature; + +Temperature::Temperature( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh, "base_link") +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + temperature_publisher_ = nh->create_publisher(topic_name, qos); + + RCLCPP_INFO(nh->get_logger(), "Succeeded to create temperature publisher"); +} + +void Temperature::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + try { + auto temp_msg = std::make_unique(); + + // Set timestamp and frame + temp_msg->header.stamp = now; + temp_msg->header.frame_id = frame_id_; + + // Read temperature from control table (address 112, 4 bytes) + // Firmware stores as float from DHT sensor + temp_msg->temperature = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.temperature.addr, + extern_control_table.temperature.length); + + // Set variance to 0 (unknown) + temp_msg->variance = 0.0; + + // Publish the message + temperature_publisher_->publish(std::move(temp_msg)); + } catch (const std::exception & e) { + RCLCPP_ERROR( + rclcpp::get_logger("temperature"), + "Exception in temperature publish: %s", e.what()); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("temperature"), + "Unknown exception in temperature publish"); + } +} diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index 55bb2e31b..7f16b2850 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -143,39 +143,7 @@ void TurtleBot3::add_sensors() { RCLCPP_INFO(this->get_logger(), "Add Sensors"); - uint8_t is_connected_bumper_1 = 0; - uint8_t is_connected_bumper_2 = 0; - uint8_t is_connected_illumination = 0; - uint8_t is_connected_ir = 0; - uint8_t is_connected_sonar = 0; - - this->declare_parameter("sensors.bumper_1"); - this->declare_parameter("sensors.bumper_2"); - this->declare_parameter("sensors.illumination"); - this->declare_parameter("sensors.ir"); - this->declare_parameter("sensors.sonar"); - - this->get_parameter_or( - "sensors.bumper_1", - is_connected_bumper_1, - 0); - this->get_parameter_or( - "sensors.bumper_2", - is_connected_bumper_2, - 0); - this->get_parameter_or( - "sensors.illumination", - is_connected_illumination, - 0); - this->get_parameter_or( - "sensors.ir", - is_connected_ir, - 0); - this->get_parameter_or( - "sensors.sonar", - is_connected_sonar, - 0); - + // Keep essential sensors for basic TurtleBot3 functionality sensors_.push_back( new sensors::BatteryState( node_handle_, @@ -188,15 +156,24 @@ void TurtleBot3::add_sensors() "magnetic_field", "imu_link")); + // Add analog pins sensor + analog_pins_sensor_ = new sensors::AnalogPins( + node_handle_, + "analog_pins"); + sensors_.push_back(analog_pins_sensor_); + + // Add temperature and humidity sensors sensors_.push_back( - new sensors::SensorState( + new sensors::Temperature( node_handle_, - "sensor_state", - is_connected_bumper_1, - is_connected_bumper_2, - is_connected_illumination, - is_connected_ir, - is_connected_sonar)); + "temperature")); + + sensors_.push_back( + new sensors::Humidity( + node_handle_, + "humidity")); + + RCLCPP_INFO(this->get_logger(), "Successfully added all sensors"); dxl_sdk_wrapper_->read_data_set(); sensors_.push_back(