diff --git a/.travis.yml b/.travis.yml index 3c9951ea..3efada3a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,36 +1,27 @@ -# This config file for Travis CI utilizes ros-industrial/industrial_ci package. -# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst - -sudo: required -dist: trusty services: - docker -language: generic -python: - - "2.7" -compiler: - - gcc + +language: + - none + notifications: email: on_success: change on_failure: always recipients: - pyo@robotis.com -env: - matrix: - - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=xenial - - ROS_DISTRO=melodic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=bionic - - ROS_DISTRO=melodic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=stretch -matrix: - allow_failures: - - env: ROS_DISTRO=melodic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=ubuntu OS_CODE_NAME=bionic - - env: ROS_DISTRO=melodic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=stretch + - thlim@robotis.com + branches: only: - - master - - develop + - ros2 + - ros2-devel + - dashing-devel + install: - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config -script: - - source .ci_config/travis.sh - + - git clone --quiet --depth 1 https://github.com/ROBOTIS-GIT/ros2ci.git .ros2ci + - cp turtlebot3_ci.repos .ros2ci/additional_repos.repos + +matrix: + include: + - script: .ros2ci/travis.bash dashing diff --git a/ISSUE_TEMPLATE.md b/ISSUE_TEMPLATE.md index 868fb8c0..5ca4cc17 100644 --- a/ISSUE_TEMPLATE.md +++ b/ISSUE_TEMPLATE.md @@ -1,4 +1,4 @@ -ISSUE TEMPLATE ver. 0.2.0 +ISSUE TEMPLATE ver. 0.3.0 1. Which TurtleBot3 you have? @@ -6,38 +6,48 @@ ISSUE TEMPLATE ver. 0.2.0 - [ ] Waffle - [ ] Waffle Pi -2. Which SBC(Single Board Computer) is working on TurtleBot3? +2. Which ROS is working with TurtleBot3? + + - [ ] ROS 1 Kinetic Kame + - [ ] ROS 1 Melodic Morenia + - [ ] ROS 2 Dashing Diademata + - [ ] ROS 2 Eloquent Elusor + - [ ] etc (PLEASE, WRITE DOWN YOUR ROS VERSION HERE) + +3. Which SBC(Single Board Computer) is working on TurtleBot3? - [ ] Raspberry Pi 3 - [ ] Intel Joule 570x - [ ] etc (PLEASE, WRITE DOWN YOUR SBC HERE) -3. Which OS you installed in SBC? +4. Which OS you installed in SBC? - - [ ] Ubuntu MATE 16.04 or later - [ ] Raspbian + - [ ] Ubuntu MATE 16.04 or later + - [ ] Ubuntu Server 18.04 or later - [ ] etc (PLEASE, WRITE DOWN YOUR OS) -4. Which OS you installed in Remote PC? +5. Which OS you installed in Remote PC? - [ ] Ubuntu 16.04 LTS (Xenial Xerus) - [ ] Ubuntu 18.04 LTS (Bionic Beaver) - [ ] Linux Mint 18.x + - [ ] Linux Mint 19.x - [ ] etc (PLEASE, WRITE DOWN YOUR OS) -5. Write down software version and firmware version +6. Write down software version and firmware version - Software version: [x.x.x] - Firmware version: [x.x.x] - -6. Write down the commands you used in order + +7. Write down the commands you used in order - HERE - -7. Copy and Paste your error message on terminal + +8. Copy and Paste your error message on terminal - HERE - -8. Please, describe detailedly what difficulty you are in + +9. Please, describe detailedly what difficulty you are in - HERE diff --git a/README.md b/README.md index c913daa3..f2a5408b 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,15 @@ # TurtleBot3 -## ROS Packages for TurtleBot3 -|Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +## ROS 1 Packages for TurtleBot3 +|develop|master|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| +|:---:|:---:|:---:|:---:| +|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=develop)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=master)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=melodic-devel)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)| + +## ROS 2 Packages for TurtleBot3 +|ros2-devel|ros2|Dashing + Ubuntu Bionic| |:---:|:---:|:---:| -|[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2Fturtlebot3.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2Fturtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=melodic-devel)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)| +|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=ros2-devel)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=ros2)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/turtlebot3.svg?branch=dashing-devel)](https://travis-ci.org/ROBOTIS-GIT/turtlebot3)| ## ROBOTIS e-Manual for TurtleBot3 - [ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/) diff --git a/turtlebot3.repos b/turtlebot3.repos index d46078c2..d26248af 100644 --- a/turtlebot3.repos +++ b/turtlebot3.repos @@ -11,39 +11,15 @@ repositories: type: git url: https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git version: ros2 - gazebo/gazebo_ros_pkgs: - type: git - url: https://github.com/ros-simulation/gazebo_ros_pkgs.git - version: ros2 - gazebo/camera_info_manager: - type: git - url: https://github.com/ros-perception/image_common.git - version: ros2 - gazebo/vision_opencv: - type: git - url: https://github.com/ros-perception/vision_opencv.git - version: ros2 - cartographer/cartographer: - type: git - url: https://github.com/ROBOTIS-GIT/cartographer.git - version: dashing - cartographer/cartographer_ros: + utils/cartographer_ros: type: git url: https://github.com/ROBOTIS-GIT/cartographer_ros.git version: dashing - cartographer/pcl_conversions: - type: git - url: https://github.com/ros2/pcl_conversions.git - version: ros2 - navigation2/navigation2: - type: git - url: https://github.com/ros-planning/navigation2.git - version: dashing-devel - navigation2/BehaviorTree.CPP: + utils/DynamixelSDK: type: git - url: https://github.com/BehaviorTree/BehaviorTree.CPP.git + url: https://github.com/ROBOTIS-GIT/DynamixelSDK.git version: ros2 - navigation2/angles: + utils/hls_lfcd_lds_driver: type: git - url: https://github.com/ros/angles.git + url: https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver.git version: ros2 diff --git a/turtlebot3/CHANGELOG.rst b/turtlebot3/CHANGELOG.rst new file mode 100644 index 00000000..56eabc09 --- /dev/null +++ b/turtlebot3/CHANGELOG.rst @@ -0,0 +1,144 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Removed ament_export_dependency(xacro) `#462 `_ +* Added use_sim_time parameter for rviz2 `#456 `_ +* Fixed ROS2 dependencies and library install `#454 `_ +* Fixed turtlebot3.repos `#447 `_ `#432 `_ +* Fixed scan rate to 5hz `#418 `_ +* Initialized joint states variables `#411 `_ +* Updated map.yaml `#386 `_ +* Contributors: Matt Hansen, Alberto Soragna, Carl Delsey, sousou1, Emerson Knapp, Mikael Arguedas, Ross Desmond, Darby Lim, Pyo + +1.2.2 (2019-08-20) +------------------ +* Fixed `dwa local planner params` for dwa_local_planner 1.16.2 `#415 `_ +* This patch only applies to ROS 1 Melodic. +* Contributors: atinfinity, Kayman + +1.2.1 (2019-08-20) +------------------ +* Fixed typo `#436 `_ +* Fixed ROS_ASSERT bug `#416 `_ +* Deleted '/' to sync tf2 `#402 `_ +* Added turtlebot3_remote.launch to turtlebot3_model.launch `#389 `_ +* Contributors: Jonathan Hechtbauer, Pallav Bhalla, ant, Ryan Shim, Kayman, Darby Lim, Gilbert, Pyo + +1.2.0 (2019-01-22) +------------------ +* changed math.ceil() operation `#373 `_ +* fixed TypeError integers +* fixed read of scanned samples when there isn't 360 +* updated map.yaml `#348 `_ +* added an additional argument move_forward_only to prohibit backward locomotion in navigation `#339 `_ +* fixed typo `#280 `_ +* added windows port `#358 `_ +* Contributors: Gilbert, Darby Lim, linzhibo, oiz5201618, yoshimalucky, Steven Macenski, Eduardo Avelar, Sean Yen, Pyo + +1.1.0 (2018-07-23) +------------------ +* added bringup to load multiple robot simply #251 +* added arguments for multiple robot +* added odometrySource +* modified camera topic name +* modified base_scan update_rate and add param on diff_drive #258 +* modified the laser scanner update_rate in the gazebo xacro files #258 +* modified origin of collision in Waffle URDF +* updated turtlebot3_diagnostic node +* updated firmware version from 1.2.0 to 1.2.2 +* updated get firmware version +* updated version check function +* updated warn msg for version check +* deleted unused get_scan function #227 +* Contributors: Darby Lim, Gilbert, Eduardo Avelar, shtseng, Pyo + +1.0.0 (2018-05-29) +------------------ +* added cartographer +* added hector mapping +* added karto SLAM +* added frontier_exploration +* added launch files to run various SLAMs +* added robot model for OpenManipulator and turtlebot3_autorace +* added exec python nodes like marker_server in catkin_install_python +* added frameName for imu on gazebo (however, there is no effect.) +* added variable to check version only once (turtlebot3_bringup) +* modified global names `#211 `_ from FurqanHabibi/fix_global_topic_name +* modified gmapping parameters +* modified navigation parameters +* modified version check and firmware version (turtlebot3_bringup) +* modified robot names +* modified range of lidar, lidar position, scan param +* modified camera position and fixed slip bug +* modified waffle_pi stl files +* modified initial value, profile function, limit velocity msg (teleop) +* merged pull request `#154 `_ `#153 `_ `#148 `_ `#147 `_ `#146 `_ `#145 `_ +* Contributors: Darby Lim, Leon Jung, Gilbert, KurtE, ncnynl, FurqanHabibi, skasperski, ihadzic, Pyo + +0.2.1 (2018-03-14) +------------------ +* added install directory +* refactoring for release +* Contributors: Pyo + +0.2.0 (2018-03-12) +------------------ +* added turtlebot3_rpicamera.launch for raspberry pi camera +* added waffle pi model (urdf and gazebo) +* added verion check function +* added diagnostics node +* added scripts for reload rules +* added example package +* modified firmware version +* modified param config +* modified topic of gazebo plugin +* modified r200 tf tree +* modified gazebo imu link +* removed the large bag file and added download command from other site +* refactoring for release +* Contributors: Darby Lim, Gilbert, Leon Jung, Pyo + +0.1.6 (2017-08-14) +------------------ +* fixed typo +* fixed xacro.py deprecation +* modified file location +* updated nav param +* updated SLAM param +* updated model.launch +* updated IMU link +* updated gazebo config +* Contributors: Darby Lim, Hunter L. Allen + +0.1.5 (2017-05-25) +------------------ +* updated turtlebot3 waffle URDF +* changed the node name from hlds_laser_publisher to turtlebot3_lds +* modified bag and map files +* added SLAM bag file +* Contributors: Darby Lim, Pyo + +0.1.4 (2017-05-23) +------------------ +* modified launch file name +* added teleop package +* Contributors: Darby Lim + +0.1.3 (2017-04-24) +------------------ +* Detached turtlebot3_msgs package from turtlebot3 package for uploading to rosdistro +* modified the package information for release +* modified SLAM param +* modified the description, authors, depend option and delete the core package +* modified the turtlebot bringup files +* modified pkg setting for turtlebot3_core +* modified the navigation package and turtlebot3 node for demo +* modified the wheel speed gain +* added Intel RealSense R200 +* added LDS sensor +* Contributors: Darby Lim, Leon Jung, Pyo diff --git a/turtlebot3/package.xml b/turtlebot3/package.xml index 155f6972..4383362b 100644 --- a/turtlebot3/package.xml +++ b/turtlebot3/package.xml @@ -1,13 +1,14 @@ - + + turtlebot3 - 1.0.0 + 2.0.0 - ROS2 packages for TurtleBot3 + ROS 2 packages for TurtleBot3 Apache 2.0 Darby Lim - Pyo + Pyo Pyo http://wiki.ros.org/turtlebot3_description http://turtlebot3.robotis.com @@ -17,7 +18,6 @@ turtlebot3_bringup turtlebot3_cartographer turtlebot3_description - turtlebot3_lidar turtlebot3_navigation2 turtlebot3_node turtlebot3_teleop diff --git a/turtlebot3_bringup/99-turtlebot3-cdc.rules b/turtlebot3_bringup/99-turtlebot3-cdc.rules new file mode 100644 index 00000000..efd71c43 --- /dev/null +++ b/turtlebot3_bringup/99-turtlebot3-cdc.rules @@ -0,0 +1,10 @@ +#http://linux-tips.org/t/prevent-modem-manager-to-capture-usb-serial-devices/284/2. + +#cp rules /etc/udev/rules.d/ +#sudo udevadm control --reload-rules +#sudo udevadm trigger + +ATTRS{idVendor}=="0483" ATTRS{idProduct}=="5740", ENV{ID_MM_DEVICE_IGNORE}="1", MODE:="0666" +ATTRS{idVendor}=="0483" ATTRS{idProduct}=="df11", MODE:="0666" +ATTRS{idVendor}=="fff1" ATTRS{idProduct}=="ff48", ENV{ID_MM_DEVICE_IGNORE}="1", MODE:="0666" +ATTRS{idVendor}=="10c4" ATTRS{idProduct}=="ea60", ENV{ID_MM_DEVICE_IGNORE}="1", MODE:="0666" \ No newline at end of file diff --git a/turtlebot3_bringup/CHANGELOG.rst b/turtlebot3_bringup/CHANGELOG.rst index 7f533ec2..2e010291 100644 --- a/turtlebot3_bringup/CHANGELOG.rst +++ b/turtlebot3_bringup/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package turtlebot3_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Fixed ROS2 dependencies and library install `#454 `_ +* Contributors: Emerson Knapp, Darby Lim, Pyo + +1.2.2 (2019-08-20) +------------------ +* none + +1.2.1 (2019-08-20) +------------------ +* Added turtlebot3_remote.launch to turtlebot3_model.launch `#389 `_ +* Contributors: Jonathan Hechtbauer, Gilbert + +1.2.0 (2019-01-22) +------------------ +* none + 1.1.0 (2018-07-23) ------------------ * added bringup to load multiple robot simply #251 diff --git a/turtlebot3_bringup/CMakeLists.txt b/turtlebot3_bringup/CMakeLists.txt index 2e4d6da0..0208aa8a 100644 --- a/turtlebot3_bringup/CMakeLists.txt +++ b/turtlebot3_bringup/CMakeLists.txt @@ -17,34 +17,11 @@ endif() ################################################################################ find_package(ament_cmake REQUIRED) -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -# catkin_package( -# CATKIN_DEPENDS roscpp std_msgs sensor_msgs diagnostic_msgs turtlebot3_msgs -# ) - -################################################################################ -# Build -################################################################################ - ################################################################################ # Install ################################################################################ install( - DIRECTORY launch + DIRECTORY launch param DESTINATION share/${PROJECT_NAME} ) @@ -52,4 +29,3 @@ install( # Macro for ament package ################################################################################ ament_package() - diff --git a/turtlebot3_bringup/launch/robot.launch.py b/turtlebot3_bringup/launch/robot.launch.py index f26ef753..a9ff378d 100644 --- a/turtlebot3_bringup/launch/robot.launch.py +++ b/turtlebot3_bringup/launch/robot.launch.py @@ -18,31 +18,65 @@ import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch.substitutions import ThisLaunchFileDir from launch_ros.actions import Node + def generate_launch_description(): + TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] + + usb_port = LaunchConfiguration('usb_port', default='/dev/ttyACM0') + + tb3_param_dir = LaunchConfiguration( + 'tb3_param_dir', + default=os.path.join( + get_package_share_directory('turtlebot3_bringup'), + 'param', + TURTLEBOT3_MODEL + '.yaml')) + + lidar_pkg_dir = LaunchConfiguration( + 'lidar_pkg_dir', + default=os.path.join(get_package_share_directory('hls_lfcd_lds_driver'), 'launch')) + use_sim_time = LaunchConfiguration('use_sim_time', default='false') return LaunchDescription([ DeclareLaunchArgument( - 'use_sim_time', + 'use_sim_time', default_value=use_sim_time, description='Use simulation (Gazebo) clock if true'), + DeclareLaunchArgument( + 'usb_port', + default_value=usb_port, + description='Connected USB port with OpenCR'), + + DeclareLaunchArgument( + 'tb3_param_dir', + default_value=tb3_param_dir, + description='Full path to turtlebot3 parameter file to load'), + IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/turtlebot3_state_publisher.launch.py']), + PythonLaunchDescriptionSource( + [ThisLaunchFileDir(), '/turtlebot3_state_publisher.launch.py']), launch_arguments={'use_sim_time': use_sim_time}.items(), ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([lidar_pkg_dir, '/hlds_laser.launch.py']), + launch_arguments={'port': '/dev/ttyUSB0', 'frame_id': 'base_scan'}.items(), + ), + Node( package='turtlebot3_node', node_executable='turtlebot3_ros', - node_name='turtlebot3_node', + parameters=[tb3_param_dir], + arguments=['-i', usb_port], output='screen'), - ]) \ No newline at end of file + ]) diff --git a/turtlebot3_bringup/launch/rviz2.launch.py b/turtlebot3_bringup/launch/rviz2.launch.py index 15f751e5..7e6987f5 100644 --- a/turtlebot3_bringup/launch/rviz2.launch.py +++ b/turtlebot3_bringup/launch/rviz2.launch.py @@ -22,15 +22,18 @@ from launch import LaunchDescription from launch_ros.actions import Node + def generate_launch_description(): - rviz_config_dir = os.path.join(get_package_share_directory('turtlebot3_description'), 'rviz', 'model.rviz') - - return LaunchDescription([ + rviz_config_dir = os.path.join( + get_package_share_directory('turtlebot3_description'), + 'rviz', + 'model.rviz') + return LaunchDescription([ Node( package='rviz2', node_executable='rviz2', node_name='rviz2', arguments=['-d', rviz_config_dir], output='screen'), - ]) \ No newline at end of file + ]) diff --git a/turtlebot3_bringup/launch/turtlebot3_state_publisher.launch.py b/turtlebot3_bringup/launch/turtlebot3_state_publisher.launch.py index 104fc822..bb822892 100644 --- a/turtlebot3_bringup/launch/turtlebot3_state_publisher.launch.py +++ b/turtlebot3_bringup/launch/turtlebot3_state_publisher.launch.py @@ -20,19 +20,24 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.substitutions import LaunchConfiguration, EnvironmentVariable from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] def generate_launch_description(): + TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] + use_sim_time = LaunchConfiguration('use_sim_time', default='false') urdf_file_name = 'turtlebot3_' + TURTLEBOT3_MODEL + '.urdf' + print("urdf_file_name : {}".format(urdf_file_name)) - urdf = os.path.join(get_package_share_directory('turtlebot3_description'), 'urdf', urdf_file_name) - + urdf = os.path.join( + get_package_share_directory('turtlebot3_description'), + 'urdf', + urdf_file_name) + return LaunchDescription([ DeclareLaunchArgument( 'use_sim_time', @@ -46,4 +51,4 @@ def generate_launch_description(): output='screen', parameters=[{'use_sim_time': use_sim_time}], arguments=[urdf]), - ]) \ No newline at end of file + ]) diff --git a/turtlebot3_bringup/package.xml b/turtlebot3_bringup/package.xml index ecc893c0..2ce53d88 100644 --- a/turtlebot3_bringup/package.xml +++ b/turtlebot3_bringup/package.xml @@ -1,9 +1,10 @@ - + + turtlebot3_bringup - 0.0.1 + 2.0.0 - ros2 launch scripts for starting the TurtleBot3 + ROS 2 launch scripts for starting the TurtleBot3 Apache 2.0 Darby Lim @@ -14,6 +15,9 @@ https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues ament_cmake + hls_lfcd_lds_driver + robot_state_publisher + rviz2 turtlebot3_description turtlebot3_node diff --git a/turtlebot3_bringup/param/burger.yaml b/turtlebot3_bringup/param/burger.yaml new file mode 100644 index 00000000..0d6445d7 --- /dev/null +++ b/turtlebot3_bringup/param/burger.yaml @@ -0,0 +1,37 @@ +turtlebot3_node: + ros__parameters: + + opencr: + id: 200 + baud_rate: 1000000 + protocol_version: 2.0 + + wheels: + separation: 0.160 + radius: 0.033 + + motors: + profile_acceleration_constant: 214.577 + + # [rev/min2] + # ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#profile-acceleration + profile_acceleration: 0.0 + + sensors: + bumper_1: false + bumper_2: false + + illumination: false + + ir: false + + sonar: false + +diff_drive_controller: + ros__parameters: + + odometry: + publish_tf: true + use_imu: true + frame_id: "odom" + child_frame_id: "base_footprint" diff --git a/turtlebot3_bringup/param/waffle.yaml b/turtlebot3_bringup/param/waffle.yaml new file mode 100644 index 00000000..4693b835 --- /dev/null +++ b/turtlebot3_bringup/param/waffle.yaml @@ -0,0 +1,37 @@ +turtlebot3_node: + ros__parameters: + + opencr: + id: 200 + baud_rate: 1000000 + protocol_version: 2.0 + + wheels: + separation: 0.287 + radius: 0.033 + + motors: + profile_acceleration_constant: 214.577 + + # [rev/min2] + # ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#profile-acceleration + profile_acceleration: 0.0 + + sensors: + bumper_1: false + bumper_2: false + + illumination: false + + ir: false + + sonar: false + +diff_drive_controller: + ros__parameters: + + odometry: + publish_tf: true + use_imu: true + frame_id: "odom" + child_frame_id: "base_footprint" diff --git a/turtlebot3_bringup/param/waffle_pi.yaml b/turtlebot3_bringup/param/waffle_pi.yaml new file mode 100644 index 00000000..4693b835 --- /dev/null +++ b/turtlebot3_bringup/param/waffle_pi.yaml @@ -0,0 +1,37 @@ +turtlebot3_node: + ros__parameters: + + opencr: + id: 200 + baud_rate: 1000000 + protocol_version: 2.0 + + wheels: + separation: 0.287 + radius: 0.033 + + motors: + profile_acceleration_constant: 214.577 + + # [rev/min2] + # ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#profile-acceleration + profile_acceleration: 0.0 + + sensors: + bumper_1: false + bumper_2: false + + illumination: false + + ir: false + + sonar: false + +diff_drive_controller: + ros__parameters: + + odometry: + publish_tf: true + use_imu: true + frame_id: "odom" + child_frame_id: "base_footprint" diff --git a/turtlebot3_cartographer/CHANGELOG.rst b/turtlebot3_cartographer/CHANGELOG.rst new file mode 100644 index 00000000..97abeaff --- /dev/null +++ b/turtlebot3_cartographer/CHANGELOG.rst @@ -0,0 +1,80 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_cartographer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Changed package name to turtlebot3_cartographer from turtlebot3_slam +* Contributors: Darby Lim, Pyo + +1.2.2 (2019-08-20) +------------------ +* none + +1.2.1 (2019-08-20) +------------------ +* Fixed ROS_ASSERT bug `#416 `_ +* Contributors: ant, Ryan Shim, Kayman + +1.2.0 (2019-01-22) +------------------ +* modified rosdep `#342 `_ +* Contributors: Steven Macenski, Darby Lim + +1.1.0 (2018-07-23) +------------------ +* added bringup to load multiple robot simply #251 +* added arguments for multiple robot +* Contributors: Darby Lim, Gilbert, Pyo + +1.0.0 (2018-05-29) +------------------ +* added cartographer +* added hector mapping +* added karto SLAM +* added frontier_exploration +* added launch files to run various SLAMs +* modified pull request `#220 `_ `#219 `_ `#215 `_ `#212 `_ `#202 `_ `#154 `_ `#153 `_ `#147 `_ `#146 `_ `#145 `_ +* Contributors: Darby Lim, ncnynl, Pyo + +0.2.1 (2018-03-14) +------------------ +* none + +0.2.0 (2018-03-12) +------------------ +* removed the large bag file and added download command from other site +* refactoring for release +* Contributors: Hunter L. Allen, Pyo + +0.1.6 (2017-08-14) +------------------ +* updated SLAM param +* Contributors: Darby Lim + +0.1.5 (2017-05-25) +------------------ +* modified bag and map files +* added SLAM bag files +* Contributors: Darby Lim, Pyo + +0.1.4 (2017-05-23) +------------------ +* modified launch file name +* added teleop package +* Contributors: Darby Lim + +0.1.3 (2017-04-24) +------------------ +* modified the package information for release +* modified SLAM param +* modified the description, authors, depend option and delete the core package +* modified the turtlebot bringup files +* modified pkg setting for turtlebot3_core +* modified the navigation package and turtlebot3 node for demo +* modified the wheel speed gain +* added Intel RealSense R200 +* added LDS sensor +* Contributors: Darby Lim, Pyo diff --git a/turtlebot3_cartographer/CMakeLists.txt b/turtlebot3_cartographer/CMakeLists.txt index 27920cbd..ca230fd2 100644 --- a/turtlebot3_cartographer/CMakeLists.txt +++ b/turtlebot3_cartographer/CMakeLists.txt @@ -11,26 +11,6 @@ find_package(ament_cmake REQUIRED) find_package(cartographer REQUIRED) find_package(cartographer_ros REQUIRED) -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ - -################################################################################ -# Build -################################################################################ - ################################################################################ # Install ################################################################################ @@ -43,4 +23,3 @@ install( # Macro for ament package ################################################################################ ament_package() - diff --git a/turtlebot3_cartographer/package.xml b/turtlebot3_cartographer/package.xml index d94fddff..362f11c0 100644 --- a/turtlebot3_cartographer/package.xml +++ b/turtlebot3_cartographer/package.xml @@ -1,13 +1,14 @@ - + + turtlebot3_cartographer - 0.0.1 + 2.0.0 - launch scripts for cartographer + ROS 2 launch scripts for cartographer Apache 2.0 Darby Lim - Pyo + Pyo Pyo http://wiki.ros.org/turtlebot3_bringup http://turtlebot3.robotis.com diff --git a/turtlebot3_ci.repos b/turtlebot3_ci.repos new file mode 100644 index 00000000..4fd85e2c --- /dev/null +++ b/turtlebot3_ci.repos @@ -0,0 +1,17 @@ +repositories: + turtlebot3/turtlebot3_msgs: + type: git + url: https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git + version: ros2 + utils/cartographer_ros: + type: git + url: https://github.com/ROBOTIS-GIT/cartographer_ros.git + version: dashing + utils/DynamixelSDK: + type: git + url: https://github.com/ROBOTIS-GIT/DynamixelSDK.git + version: ros2 + utils/hls_lfcd_lds_driver: + type: git + url: https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver.git + version: ros2 diff --git a/turtlebot3_description/CHANGELOG.rst b/turtlebot3_description/CHANGELOG.rst new file mode 100644 index 00000000..ebc3f6aa --- /dev/null +++ b/turtlebot3_description/CHANGELOG.rst @@ -0,0 +1,88 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Removed ament_export_dependency(xacro) `#462 `_ +* Contributors: Ross Desmond, Darby Lim, Pyo + +1.2.2 (2019-08-20) +------------------ +* none + +1.2.1 (2019-08-20) +------------------ +* none + +1.2.0 (2019-01-22) +------------------ +* none + +1.1.0 (2018-07-23) +------------------ +* added bringup to load multiple robot simply #251 +* added odometrySource +* modified camera topic name +* modified base_scan update_rate and add param on diff_drive #258 +* modified the laser scanner update_rate in the gazebo xacro files #258 +* modified origin of collision in Waffle URDF +* Contributors: Darby Lim, Gilbert, shtseng, Pyo + +1.0.0 (2018-05-29) +------------------ +* added frameName for imu on gazebo (however, there is no effect.) +* modified robot names +* modified range of lidar, lidar position, scan param +* modified camera position and fixed slip bug +* modified waffle_pi stl files +* merged pull request `#220 `_ `#212 `_ `#200 `_ `#155 `_ `#154 `_ `#153 `_ `#147 `_ `#146 `_ +* Contributors: Darby Lim, Pyo + +0.2.1 (2018-03-14) +------------------ +* refactoring for release +* Contributors: Pyo + +0.2.0 (2018-03-12) +------------------ +* added waffle pi model (urdf and gazebo) +* modified topic of gazebo plugin +* refactoring for release +* modified r200 tf tree +* modified gazebo imu link +* Contributors: Darby Lim, Pyo + +0.1.6 (2017-08-14) +------------------ +* modified models +* fixed xacro.py deprecation +* updated IMU link +* updated gazebo config +* Contributors: Darby Lim, Hunter L. Allen + +0.1.5 (2017-05-25) +------------------ +* updated turtlebot3 waffle URDF +* Contributors: Darby Lim + +0.1.4 (2017-05-23) +------------------ +* modified launch file name +* added teleop package +* Contributors: Darby Lim + +0.1.3 (2017-04-24) +------------------ +* modified the package information for release +* modified SLAM param +* modified the description, authors, depend option and delete the core package +* modified the turtlebot bringup files +* modified pkg setting for turtlebot3_core +* modified the navigation package and turtlebot3 node for demo +* modified the wheel speed gain +* added Intel RealSense R200 +* added LDS sensor +* Contributors: Darby Lim, Pyo diff --git a/turtlebot3_description/CMakeLists.txt b/turtlebot3_description/CMakeLists.txt index c80bcf93..5a2d2d98 100644 --- a/turtlebot3_description/CMakeLists.txt +++ b/turtlebot3_description/CMakeLists.txt @@ -18,26 +18,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(urdf REQUIRED) -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ - -################################################################################ -# Build -################################################################################ - ################################################################################ # Install ################################################################################ diff --git a/turtlebot3_description/package.xml b/turtlebot3_description/package.xml index ccf81d57..bf2ba694 100644 --- a/turtlebot3_description/package.xml +++ b/turtlebot3_description/package.xml @@ -1,13 +1,14 @@ - + + turtlebot3_description - 0.0.1 + 2.0.0 3D models of the TurtleBot3 for simulation and visualization Apache 2.0 Darby Lim - Pyo + Pyo Pyo http://wiki.ros.org/turtlebot3_description http://turtlebot3.robotis.com diff --git a/turtlebot3_description/urdf/turtlebot3_burger.urdf b/turtlebot3_description/urdf/turtlebot3_burger.urdf index e9a7854f..a418b005 100644 --- a/turtlebot3_description/urdf/turtlebot3_burger.urdf +++ b/turtlebot3_description/urdf/turtlebot3_burger.urdf @@ -1,9 +1,9 @@ - - + diff --git a/turtlebot3_description/urdf/turtlebot3_waffle.urdf b/turtlebot3_description/urdf/turtlebot3_waffle.urdf index e2a1d26d..e591079d 100644 --- a/turtlebot3_description/urdf/turtlebot3_waffle.urdf +++ b/turtlebot3_description/urdf/turtlebot3_waffle.urdf @@ -7,7 +7,7 @@ --> - + @@ -71,7 +71,7 @@ - + diff --git a/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf b/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf index 08fce8d8..03ff47f5 100644 --- a/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf +++ b/turtlebot3_description/urdf/turtlebot3_waffle_pi.urdf @@ -2,7 +2,7 @@ - + diff --git a/turtlebot3_navigation2/CHANGELOG.rst b/turtlebot3_navigation2/CHANGELOG.rst new file mode 100644 index 00000000..f5f2824c --- /dev/null +++ b/turtlebot3_navigation2/CHANGELOG.rst @@ -0,0 +1,85 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_navigation2 +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Added use_sim_time parameter for rviz2 `#456 `_ +* Updated map.yaml `#386 `_ +* Changed package name to turtlebot3_navigation2, it supoorts Nav2 of ROS 2 +* Contributors: Mikael Arguedas, Alberto Soragna, Darby Lim, Pyo + +1.2.2 (2019-08-20) +------------------ +* Fixed `dwa local planner params` for dwa_local_planner 1.16.2 `#415 `_ +* This patch only applies to ROS 1 Melodic. +* Contributors: atinfinity, Kayman + +1.2.1 (2019-08-20) +------------------ +* Deleted '/' to sync tf2 `#402 `_ +* Contributors: Ryan Shim, Kayman, Darby Lim + +1.2.0 (2019-01-22) +------------------ +* updated map.yaml `#348 `_ +* added an additional argument move_forward_only to prohibit backward locomotion in navigation `#339 `_ +* fixed typo `#280 `_ +* Contributors: Gilbert, Darby Lim, linzhibo, oiz5201618, yoshimalucky, Pyo + +1.1.0 (2018-07-23) +------------------ +* added bringup to load multiple robot simply #251 +* added arguments for multiple robot +* Contributors: Darby Lim, Gilbert, Pyo + +1.0.0 (2018-05-29) +------------------ +* modified navigation parameters +* modified control frequency and inflation radius +* modified param after experiments +* merged pull request `#202 `_ from ROBOTIS-GIT/feature-cartographer +* merged pull request `#220 `_ `#212 `_ `#200 `_ `#154 `_ `#153 `_ `#147 `_ `#146 `_ +* Contributors: Darby Lim, Pyo + +0.2.1 (2018-03-14) +------------------ +* refactoring for release +* Contributors: Pyo + +0.2.0 (2018-03-12) +------------------ +* added waffle pi model (urdf and gazebo) +* refactoring for release +* Contributors: Darby Lim, Pyo + +0.1.6 (2017-08-14) +------------------ +* updated nav param +* Contributors: Darby Lim + +0.1.5 (2017-05-25) +------------------ +* modified bag and map files +* Contributors: Darby Lim, Pyo + +0.1.4 (2017-05-23) +------------------ +* modified launch file name +* added teleop package +* Contributors: Darby Lim + +0.1.3 (2017-04-24) +------------------ +* modified the package information for release +* modified SLAM param +* modified the description, authors, depend option and delete the core package +* modified the turtlebot bringup files +* modified pkg setting for turtlebot3_core +* modified the navigation package and turtlebot3 node for demo +* modified the wheel speed gain +* added Intel RealSense R200 +* added LDS sensor +* Contributors: Darby Lim, Leon Jung, Pyo diff --git a/turtlebot3_navigation2/CMakeLists.txt b/turtlebot3_navigation2/CMakeLists.txt index c2607a75..9f008dd6 100644 --- a/turtlebot3_navigation2/CMakeLists.txt +++ b/turtlebot3_navigation2/CMakeLists.txt @@ -10,26 +10,6 @@ project(turtlebot3_navigation2) find_package(ament_cmake REQUIRED) find_package(navigation2 REQUIRED) -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ - -################################################################################ -# Build -################################################################################ - ################################################################################ # Install ################################################################################ @@ -42,4 +22,3 @@ install( # Macro for ament package ################################################################################ ament_package() - diff --git a/turtlebot3_navigation2/launch/navigation2.launch.py b/turtlebot3_navigation2/launch/navigation2.launch.py index f5907181..49bd950d 100644 --- a/turtlebot3_navigation2/launch/navigation2.launch.py +++ b/turtlebot3_navigation2/launch/navigation2.launch.py @@ -15,50 +15,63 @@ # /* Author: Darby Lim */ import os + from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import ThisLaunchFileDir -from launch.actions import ExecuteProcess +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') - map_dir = LaunchConfiguration('map', - default=os.path.join(get_package_share_directory('turtlebot3_navigation2'), 'map', 'map.yaml')) + map_dir = LaunchConfiguration( + 'map', + default=os.path.join( + get_package_share_directory('turtlebot3_navigation2'), + 'map', + 'map.yaml')) + + param_file_name = TURTLEBOT3_MODEL + '.yaml' + param_dir = LaunchConfiguration( + 'params', + default=os.path.join( + get_package_share_directory('turtlebot3_navigation2'), + 'param', + param_file_name)) - param_file_name = TURTLEBOT3_MODEL + '_params.yaml' - param_dir = LaunchConfiguration('params', - default=os.path.join(get_package_share_directory('turtlebot3_navigation2'), 'param', param_file_name)) - nav2_launch_file_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch') - rviz_config_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch', 'nav2_default_view.rviz') + rviz_config_dir = os.path.join( + get_package_share_directory('nav2_bringup'), + 'launch', + 'nav2_default_view.rviz') return LaunchDescription([ DeclareLaunchArgument( - 'map', + 'map', default_value=map_dir, description='Full path to map file to load'), DeclareLaunchArgument( - 'params', + 'params', default_value=param_dir, description='Full path to param file to load'), DeclareLaunchArgument( - 'use_sim_time', - default_value='false', + 'use_sim_time', + default_value='false', description='Use simulation (Gazebo) clock if true'), IncludeLaunchDescription( PythonLaunchDescriptionSource([nav2_launch_file_dir, '/nav2_bringup_launch.py']), - launch_arguments={'map': map_dir, 'use_sim_time': use_sim_time, 'params': param_dir}.items(), + launch_arguments={ + 'map': map_dir, + 'use_sim_time': use_sim_time, + 'params': param_dir}.items(), ), Node( diff --git a/turtlebot3_navigation2/package.xml b/turtlebot3_navigation2/package.xml index 62ea7e6a..5fc792e5 100644 --- a/turtlebot3_navigation2/package.xml +++ b/turtlebot3_navigation2/package.xml @@ -1,21 +1,20 @@ - + + turtlebot3_navigation2 - 0.0.1 + 2.0.0 - launch scripts for navigation2 + ROS 2 launch scripts for navigation2 Apache 2.0 Darby Lim - Pyo + Pyo Pyo http://wiki.ros.org/turtlebot3_bringup http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues ament_cmake - ament_index_python - launch navigation2 ament_cmake diff --git a/turtlebot3_navigation2/param/burger_params.yaml b/turtlebot3_navigation2/param/burger.yaml similarity index 87% rename from turtlebot3_navigation2/param/burger_params.yaml rename to turtlebot3_navigation2/param/burger.yaml index 9335a8b6..1c97c3d9 100644 --- a/turtlebot3_navigation2/param/burger_params.yaml +++ b/turtlebot3_navigation2/param/burger.yaml @@ -1,6 +1,6 @@ amcl: ros__parameters: - use_sim_time: True + use_sim_time: False alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -40,20 +40,20 @@ amcl: amcl_map_client: ros__parameters: - use_sim_time: True + use_sim_time: False amcl_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False bt_navigator: ros__parameters: - use_sim_time: True + use_sim_time: False bt_xml_filename: "bt_navigator.xml" dwb_controller: ros__parameters: - use_sim_time: True + use_sim_time: False debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 @@ -83,8 +83,8 @@ dwb_controller: transform_tolerance: 0.2 critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - GoalAlign.scale: 24.0 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 PathDist.scale: 32.0 GoalDist.scale: 24.0 RotateToGoal.scale: 32.0 @@ -92,7 +92,7 @@ dwb_controller: local_costmap: local_costmap: ros__parameters: - use_sim_time: True + use_sim_time: False global_frame: odom plugin_names: ["obstacle_layer", "inflation_layer"] plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] @@ -113,15 +113,15 @@ local_costmap: marking: True local_costmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False local_costmap_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False global_costmap: global_costmap: ros__parameters: - use_sim_time: True + use_sim_time: False robot_radius: 0.105 obstacle_layer: enabled: True @@ -134,19 +134,19 @@ global_costmap: marking: True global_costmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False global_costmap_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False map_server: ros__parameters: - use_sim_time: True + use_sim_time: False yaml_filename: "map.yaml" lifecycle_manager: ros__parameters: - use_sim_time: True + use_sim_time: False autostart: True node_names: ['map_server', 'amcl', 'world_model', 'dwb_controller', @@ -154,26 +154,26 @@ lifecycle_manager: lifecycle_manager_service_client: ros__parameters: - use_sim_time: True + use_sim_time: False lifecycle_manager_client_service_client: ros__parameters: - use_sim_time: True + use_sim_time: False navfn_planner: ros__parameters: - use_sim_time: True + use_sim_time: False tolerance: 0.0 use_astar: false navfn_planner_GetCostmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False robot_state_publisher: ros__parameters: - use_sim_time: True + use_sim_time: False world_model: ros__parameters: - use_sim_time: True + use_sim_time: False diff --git a/turtlebot3_navigation2/param/waffle_params.yaml b/turtlebot3_navigation2/param/waffle.yaml similarity index 85% rename from turtlebot3_navigation2/param/waffle_params.yaml rename to turtlebot3_navigation2/param/waffle.yaml index 5437a0d6..1c0719ed 100644 --- a/turtlebot3_navigation2/param/waffle_params.yaml +++ b/turtlebot3_navigation2/param/waffle.yaml @@ -1,6 +1,6 @@ amcl: ros__parameters: - use_sim_time: True + use_sim_time: False alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -40,20 +40,20 @@ amcl: amcl_map_client: ros__parameters: - use_sim_time: True + use_sim_time: False amcl_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False bt_navigator: ros__parameters: - use_sim_time: True + use_sim_time: False bt_xml_filename: "bt_navigator.xml" dwb_controller: ros__parameters: - use_sim_time: True + use_sim_time: False debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 @@ -83,8 +83,8 @@ dwb_controller: transform_tolerance: 0.2 critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - GoalAlign.scale: 24.0 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 PathDist.scale: 32.0 GoalDist.scale: 24.0 RotateToGoal.scale: 32.0 @@ -92,7 +92,7 @@ dwb_controller: local_costmap: local_costmap: ros__parameters: - use_sim_time: True + use_sim_time: False global_frame: odom plugin_names: ["obstacle_layer", "inflation_layer"] plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] @@ -100,7 +100,7 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - robot_radius: 0.220 + robot_radius: 0.22 inflation_layer.cost_scaling_factor: 3.0 obstacle_layer: enabled: True @@ -113,16 +113,16 @@ local_costmap: marking: True local_costmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False local_costmap_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False global_costmap: global_costmap: ros__parameters: - use_sim_time: True - robot_radius: 0.220 + use_sim_time: False + robot_radius: 0.22 obstacle_layer: enabled: True always_send_full_costmap: True @@ -134,19 +134,19 @@ global_costmap: marking: True global_costmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False global_costmap_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False map_server: ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" + use_sim_time: False + yaml_filename: "map.yaml" lifecycle_manager: ros__parameters: - use_sim_time: True + use_sim_time: False autostart: True node_names: ['map_server', 'amcl', 'world_model', 'dwb_controller', @@ -154,26 +154,26 @@ lifecycle_manager: lifecycle_manager_service_client: ros__parameters: - use_sim_time: True + use_sim_time: False lifecycle_manager_client_service_client: ros__parameters: - use_sim_time: True + use_sim_time: False navfn_planner: ros__parameters: - use_sim_time: True + use_sim_time: False tolerance: 0.0 use_astar: false navfn_planner_GetCostmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False robot_state_publisher: ros__parameters: - use_sim_time: True + use_sim_time: False world_model: ros__parameters: - use_sim_time: True + use_sim_time: False diff --git a/turtlebot3_navigation2/param/waffle_pi_params.yaml b/turtlebot3_navigation2/param/waffle_pi.yaml similarity index 85% rename from turtlebot3_navigation2/param/waffle_pi_params.yaml rename to turtlebot3_navigation2/param/waffle_pi.yaml index 5437a0d6..1c0719ed 100644 --- a/turtlebot3_navigation2/param/waffle_pi_params.yaml +++ b/turtlebot3_navigation2/param/waffle_pi.yaml @@ -1,6 +1,6 @@ amcl: ros__parameters: - use_sim_time: True + use_sim_time: False alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 @@ -40,20 +40,20 @@ amcl: amcl_map_client: ros__parameters: - use_sim_time: True + use_sim_time: False amcl_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False bt_navigator: ros__parameters: - use_sim_time: True + use_sim_time: False bt_xml_filename: "bt_navigator.xml" dwb_controller: ros__parameters: - use_sim_time: True + use_sim_time: False debug_trajectory_details: True min_vel_x: 0.0 min_vel_y: 0.0 @@ -83,8 +83,8 @@ dwb_controller: transform_tolerance: 0.2 critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - GoalAlign.scale: 24.0 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 PathDist.scale: 32.0 GoalDist.scale: 24.0 RotateToGoal.scale: 32.0 @@ -92,7 +92,7 @@ dwb_controller: local_costmap: local_costmap: ros__parameters: - use_sim_time: True + use_sim_time: False global_frame: odom plugin_names: ["obstacle_layer", "inflation_layer"] plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"] @@ -100,7 +100,7 @@ local_costmap: width: 3 height: 3 resolution: 0.05 - robot_radius: 0.220 + robot_radius: 0.22 inflation_layer.cost_scaling_factor: 3.0 obstacle_layer: enabled: True @@ -113,16 +113,16 @@ local_costmap: marking: True local_costmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False local_costmap_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False global_costmap: global_costmap: ros__parameters: - use_sim_time: True - robot_radius: 0.220 + use_sim_time: False + robot_radius: 0.22 obstacle_layer: enabled: True always_send_full_costmap: True @@ -134,19 +134,19 @@ global_costmap: marking: True global_costmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False global_costmap_rclcpp_node: ros__parameters: - use_sim_time: True + use_sim_time: False map_server: ros__parameters: - use_sim_time: True - yaml_filename: "turtlebot3_world.yaml" + use_sim_time: False + yaml_filename: "map.yaml" lifecycle_manager: ros__parameters: - use_sim_time: True + use_sim_time: False autostart: True node_names: ['map_server', 'amcl', 'world_model', 'dwb_controller', @@ -154,26 +154,26 @@ lifecycle_manager: lifecycle_manager_service_client: ros__parameters: - use_sim_time: True + use_sim_time: False lifecycle_manager_client_service_client: ros__parameters: - use_sim_time: True + use_sim_time: False navfn_planner: ros__parameters: - use_sim_time: True + use_sim_time: False tolerance: 0.0 use_astar: false navfn_planner_GetCostmap_client: ros__parameters: - use_sim_time: True + use_sim_time: False robot_state_publisher: ros__parameters: - use_sim_time: True + use_sim_time: False world_model: ros__parameters: - use_sim_time: True + use_sim_time: False diff --git a/turtlebot3_node/CHANGELOG.rst b/turtlebot3_node/CHANGELOG.rst new file mode 100644 index 00000000..8eb9098a --- /dev/null +++ b/turtlebot3_node/CHANGELOG.rst @@ -0,0 +1,12 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_node +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Fixed ROS2 dependencies and library install `#454 `_ +* Fixed scan rate to 5hz `#418 `_ +* Initialized joint states variables `#411 `_ +* Contributors: Matt Hansen, Emerson Knapp, Darby Lim, Pyo diff --git a/turtlebot3_node/CMakeLists.txt b/turtlebot3_node/CMakeLists.txt index 1da2e7f3..fa428fc9 100644 --- a/turtlebot3_node/CMakeLists.txt +++ b/turtlebot3_node/CMakeLists.txt @@ -16,77 +16,80 @@ endif() # Find ament packages and libraries for ament and system dependencies ################################################################################ find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) +find_package(dynamixel_sdk REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) find_package(nav_msgs REQUIRED) -find_package(turtlebot3_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rcutils REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) -find_package(builtin_interfaces REQUIRED) - -################################################################################ -# Setup for python modules and scripts -################################################################################ +find_package(turtlebot3_msgs REQUIRED) ################################################################################ -# Declare ROS messages, services and actions +# Build ################################################################################ +include_directories( + include +) -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ +add_library(${PROJECT_NAME}_lib + "src/devices/motor_power.cpp" + "src/devices/sound.cpp" + "src/devices/reset.cpp" -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ - -################################################################################ -# Build -################################################################################ -add_library(${PROJECT_NAME}_LIB SHARED - "src/joint_states.cpp" + "src/diff_drive_controller.cpp" + "src/dynamixel_sdk_wrapper.cpp" "src/odometry.cpp" + "src/turtlebot3.cpp" + + "src/sensors/battery_state.cpp" + "src/sensors/imu.cpp" + "src/sensors/joint_state.cpp" + "src/sensors/sensor_state.cpp" ) set(DEPENDENCIES - "rclcpp" - "std_msgs" - "sensor_msgs" + "dynamixel_sdk" "geometry_msgs" + "message_filters" "nav_msgs" - "turtlebot3_msgs" - "tf2" + "rclcpp" + "rcutils" + "sensor_msgs" + "std_msgs" + "std_srvs" "tf2_ros" - "builtin_interfaces" + "tf2" + "turtlebot3_msgs" ) -target_link_libraries(${PROJECT_NAME}_LIB) -ament_target_dependencies(${PROJECT_NAME}_LIB ${DEPENDENCIES}) +target_link_libraries(${PROJECT_NAME}_lib) +ament_target_dependencies(${PROJECT_NAME}_lib ${DEPENDENCIES}) set(EXECUTABLE_NAME "turtlebot3_ros") add_executable(${EXECUTABLE_NAME} src/node_main.cpp) -target_link_libraries(${EXECUTABLE_NAME} ${PROJECT_NAME}_LIB) -ament_target_dependencies(${EXECUTABLE_NAME} ${dependencies}) +target_link_libraries(${EXECUTABLE_NAME} ${PROJECT_NAME}_lib) +ament_target_dependencies(${EXECUTABLE_NAME} ${DEPENDENCIES}) ################################################################################ # Install ################################################################################ -install(TARGETS - ${PROJECT_NAME}_LIB - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin +install(DIRECTORY param + DESTINATION share/${PROJECT_NAME} ) -install(TARGETS - ${EXECUTABLE_NAME} +install(TARGETS ${EXECUTABLE_NAME} DESTINATION lib/${PROJECT_NAME} ) ################################################################################ # Macro for ament package ################################################################################ +ament_export_include_directories(include) ament_package() diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp new file mode 100644 index 00000000..55130b83 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -0,0 +1,116 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_CONTROL_TABLE_HPP_ +#define TURTLEBOT3_NODE_CONTROL_TABLE_HPP_ + +#include + +namespace robotis +{ +namespace turtlebot3 +{ +constexpr uint8_t EEPROM = 1; +constexpr uint8_t RAM = 2; + +constexpr uint8_t READ = 1; +constexpr uint8_t READ_WRITE = 3; + +typedef struct +{ + uint16_t addr; + uint8_t memory; + uint16_t length; + uint8_t rw; +} ControlItem; + +typedef struct +{ + ControlItem model_number = {0, EEPROM, 2, READ}; + ControlItem model_information = {2, EEPROM, 4, READ}; + ControlItem firmware_version = {6, EEPROM, 1, READ}; + ControlItem id = {7, EEPROM, 1, READ}; + ControlItem baud_rate = {8, EEPROM, 1, READ}; + + ControlItem millis = {10, RAM, 4, READ}; + ControlItem micros = {14, RAM, 4, READ}; + + 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 button_1 = {26, RAM, 1, READ}; + ControlItem button_2 = {27, RAM, 1, READ}; + + 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 battery_voltage = {42, RAM, 4, READ}; + ControlItem battery_percentage = {46, RAM, 4, READ}; + + ControlItem sound = {50, RAM, 1, READ_WRITE}; + + ControlItem imu_re_calibration = {59, RAM, 1, READ_WRITE}; + + ControlItem imu_angular_velocity_x = {60, RAM, 4, READ}; + ControlItem imu_angular_velocity_y = {64, RAM, 4, READ}; + ControlItem imu_angular_velocity_z = {68, RAM, 4, READ}; + ControlItem imu_linear_acceleration_x = {72, RAM, 4, READ}; + ControlItem imu_linear_acceleration_y = {76, RAM, 4, READ}; + ControlItem imu_linear_acceleration_z = {80, RAM, 4, READ}; + ControlItem imu_magnetic_x = {84, RAM, 4, READ}; + ControlItem imu_magnetic_y = {88, RAM, 4, READ}; + ControlItem imu_magnetic_z = {92, RAM, 4, READ}; + ControlItem imu_orientation_w = {96, RAM, 4, READ}; + ControlItem imu_orientation_x = {100, RAM, 4, READ}; + ControlItem imu_orientation_y = {104, RAM, 4, READ}; + ControlItem imu_orientation_z = {108, 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}; + ControlItem present_velocity_right = {132, RAM, 4, READ}; + 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}; + + ControlItem cmd_velocity_linear_x = {150, RAM, 4, READ_WRITE}; + ControlItem cmd_velocity_linear_y = {154, RAM, 4, READ_WRITE}; + ControlItem cmd_velocity_linear_z = {158, RAM, 4, READ_WRITE}; + ControlItem cmd_velocity_angular_x = {162, RAM, 4, READ_WRITE}; + ControlItem cmd_velocity_angular_y = {166, RAM, 4, READ_WRITE}; + ControlItem cmd_velocity_angular_z = {170, RAM, 4, READ_WRITE}; + + ControlItem profile_acceleration_left = {174, RAM, 4, READ_WRITE}; + ControlItem profile_acceleration_right = {178, RAM, 4, READ_WRITE}; +} ControlTable; + +const ControlTable extern_control_table; +} // turtlebot3 +} // robotis + +#endif // TURTLEBOT3_NODE_CONTROL_TABLE_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/devices/devices.hpp b/turtlebot3_node/include/turtlebot3_node/devices/devices.hpp new file mode 100644 index 00000000..6161f9f5 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/devices/devices.hpp @@ -0,0 +1,58 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_DEVICES_DEVICES_HPP_ +#define TURTLEBOT3_NODE_DEVICES_DEVICES_HPP_ + +#include +#include +#include + +#include + +#include "turtlebot3_node/control_table.hpp" +#include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +extern const ControlTable extern_control_table; +namespace devices +{ +class Devices{ + public: + explicit Devices( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper) + : nh_(nh), + dxl_sdk_wrapper_(dxl_sdk_wrapper) + { + } + + virtual void command(const void * request, void * response) = 0; + + protected: + std::shared_ptr nh_; + std::shared_ptr dxl_sdk_wrapper_; + rclcpp::QoS qos_ = rclcpp::QoS(rclcpp::ServicesQoS()); +}; +} // devices +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_DEVICES_DEVICES_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/devices/motor_power.hpp b/turtlebot3_node/include/turtlebot3_node/devices/motor_power.hpp new file mode 100644 index 00000000..ab0b9af5 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/devices/motor_power.hpp @@ -0,0 +1,52 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_DEVICES_MOTOR_POWER_HPP_ +#define TURTLEBOT3_NODE_DEVICES_MOTOR_POWER_HPP_ + +#include + +#include "turtlebot3_node/devices/devices.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace devices +{ +class MotorPower : public Devices +{ + public: + static void request( + rclcpp::Client::SharedPtr client, + std_srvs::srv::SetBool::Request req); + + explicit MotorPower( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper, + const std::string & server_name = "motor_power"); + + void command(const void * request, void * response) override; + + private: + rclcpp::Service::SharedPtr srv_; +}; +} // devices +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_DEVICES_MOTOR_POWER_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/devices/reset.hpp b/turtlebot3_node/include/turtlebot3_node/devices/reset.hpp new file mode 100644 index 00000000..f83dc3a3 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/devices/reset.hpp @@ -0,0 +1,52 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_DEVICES_RESET_HPP_ +#define TURTLEBOT3_NODE_DEVICES_RESET_HPP_ + +#include + +#include "turtlebot3_node/devices/devices.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace devices +{ +class Reset : public Devices +{ + public: + static void request( + rclcpp::Client::SharedPtr client, + std_srvs::srv::Trigger::Request req); + + explicit Reset( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper, + const std::string & server_name = "reset"); + + void command(const void * request, void * response) override; + + private: + rclcpp::Service::SharedPtr srv_; +}; +} // devices +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_DEVICES_SOUND_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp b/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp new file mode 100644 index 00000000..6fc8a73d --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp @@ -0,0 +1,52 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_DEVICES_SOUND_HPP_ +#define TURTLEBOT3_NODE_DEVICES_SOUND_HPP_ + +#include + +#include "turtlebot3_node/devices/devices.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace devices +{ +class Sound : public Devices +{ + public: + static void request( + rclcpp::Client::SharedPtr client, + turtlebot3_msgs::srv::Sound::Request req); + + explicit Sound( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper, + const std::string & server_name = "sound"); + + void command(const void * request, void * response) override; + + private: + rclcpp::Service::SharedPtr srv_; +}; +} // devices +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_DEVICES_SOUND_HPP_ diff --git a/turtlebot3_node/src/joint_state.h b/turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp similarity index 58% rename from turtlebot3_node/src/joint_state.h rename to turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp index 5165c78f..0a7e89d4 100644 --- a/turtlebot3_node/src/joint_state.h +++ b/turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp @@ -16,35 +16,29 @@ /* Author: Darby Lim */ -#ifndef TURTLEBOT3_JOINT_STATE_H -#define TURTLEBOT3_JOINT_STATE_H +#ifndef TURTLEBOT3_NODE_DIFF_DRIVE_CONTROLLER_HPP_ +#define TURTLEBOT3_NODE_DIFF_DRIVE_CONTROLLER_HPP_ #include -#include -#include -#include "rclcpp/time.hpp" +#include -#include "turtlebot3_msgs/msg/sensor_state.hpp" -#include "sensor_msgs/msg/joint_state.hpp" +#include "turtlebot3_node/odometry.hpp" +namespace robotis +{ namespace turtlebot3 { -class JointState +class DiffDriveController : public rclcpp::Node { public: - JointState(){}; - virtual ~JointState(){}; - - sensor_msgs::msg::JointState getJointState(const rclcpp::Time now); - void updateRadianFromTick(const turtlebot3_msgs::msg::SensorState::SharedPtr state); + explicit DiffDriveController(const float wheel_seperation, const float wheel_radius); + virtual ~DiffDriveController(){}; private: - std::array last_rad_ = {0.0, 0.0}; - std::array last_diff_tick_ = {0, 0}; - - std::mutex mutex_; + std::shared_ptr nh_; + std::unique_ptr odometry_; }; -} - -#endif //TURTLEBOT3_JOINT_STATE_H +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_DIFF_DRIVE_CONTROLLER_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/dynamixel_sdk_wrapper.hpp b/turtlebot3_node/include/turtlebot3_node/dynamixel_sdk_wrapper.hpp new file mode 100644 index 00000000..414705af --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/dynamixel_sdk_wrapper.hpp @@ -0,0 +1,147 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_DYNAMIXEL_SDK_WRAPPER_HPP_ +#define TURTLEBOT3_NODE_DYNAMIXEL_SDK_WRAPPER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#define LOG_INFO RCUTILS_LOG_INFO_NAMED +#define LOG_WARN RCUTILS_LOG_WARN_NAMED +#define LOG_ERROR RCUTILS_LOG_ERROR_NAMED +#define LOG_DEBUG RCUTILS_LOG_DEBUG_NAMED + +#define READ_DATA_SIZE 200 + +namespace robotis +{ +namespace turtlebot3 +{ + + +class DynamixelSDKWrapper +{ + public: + typedef struct + { + std::string usb_port; + uint8_t id; + uint32_t baud_rate; + float protocol_version; + } Device; + + explicit DynamixelSDKWrapper(const Device & device); + virtual ~DynamixelSDKWrapper(); + + template + DataByteT get_data_from_device(const uint16_t & addr, const uint16_t & length) + { + DataByteT data = 0; + uint8_t * p_data = (uint8_t*)&data; + uint16_t index = addr - read_memory_.start_addr; + + std::lock_guard lock(read_data_mutex_); + switch (length) + { + case 1: + p_data[0] = read_memory_.data[index+0]; + break; + + case 2: + p_data[0] = read_memory_.data[index+0]; + p_data[1] = read_memory_.data[index+1]; + break; + + case 4: + p_data[0] = read_memory_.data[index+0]; + p_data[1] = read_memory_.data[index+1]; + p_data[2] = read_memory_.data[index+2]; + p_data[3] = read_memory_.data[index+3]; + break; + + default: + p_data[0] = read_memory_.data[index+0]; + break; + } + + return data; + } + + bool set_data_to_device( + const uint16_t & addr, + const uint16_t & length, + uint8_t * get_data, + std::string * msg); + + void init_read_memory(const uint16_t & start_addr, const uint16_t & length); + void read_data_set(); + + bool is_connected_to_device(); + + private: + bool init_dynamixel_sdk_handlers(); + + bool read_register( + uint8_t id, + uint16_t address, + uint16_t length, + uint8_t * data_basket, + const char ** log = NULL); + + bool write_register( + uint8_t id, + uint16_t address, + uint16_t length, + uint8_t * data, + const char ** log = NULL); + + dynamixel::PortHandler * portHandler_; + dynamixel::PacketHandler * packetHandler_; + + Device device_; + + uint8_t read_data_[READ_DATA_SIZE] = {0, }; + uint8_t read_data_buffer_[READ_DATA_SIZE] = {0, }; + + typedef struct + { + uint16_t start_addr; + uint16_t length; + uint8_t * data; + } Memory; + + Memory read_memory_; + + std::mutex sdk_mutex_; + std::mutex read_data_mutex_; + std::mutex write_data_mutex_; +}; +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_DYNAMIXEL_SDK_WRAPPER_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/odometry.hpp b/turtlebot3_node/include/turtlebot3_node/odometry.hpp new file mode 100644 index 00000000..1c580d95 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/odometry.hpp @@ -0,0 +1,98 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_ODOMETRY_HPP_ +#define TURTLEBOT3_NODE_ODOMETRY_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace robotis +{ +namespace turtlebot3 +{ +class Odometry +{ + public: + explicit Odometry( + std::shared_ptr & nh, + const double wheels_separation, + const double wheels_radius); + virtual ~Odometry(){}; + + private: + bool calculate_odometry(const rclcpp::Duration & duration); + + void update_imu(const std::shared_ptr & imu); + void update_joint_state(const std::shared_ptr & joint_state); + + void joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state_msg); + + void joint_state_and_imu_callback( + const std::shared_ptr & joint_state_msg, + const std::shared_ptr & imu_msg); + + void publish(const rclcpp::Time & now); + + std::shared_ptr nh_; + std::unique_ptr tf_broadcaster_; + + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + + std::shared_ptr< + message_filters::Subscriber> msg_ftr_joint_state_sub_; + std::shared_ptr> msg_ftr_imu_sub_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::JointState, + sensor_msgs::msg::Imu> SyncPolicyJointStateImu; + typedef message_filters::Synchronizer SynchronizerJointStateImu; + + std::shared_ptr joint_state_imu_sync_; + + double wheels_separation_; + double wheels_radius_; + + std::string frame_id_of_odometry_; + std::string child_frame_id_of_odometry_; + + bool use_imu_; + bool publish_tf_; + + std::array diff_joint_positions_; + double imu_angle_; + + std::array robot_pose_; + std::array robot_vel_; +}; +} // turtlebot3 +} // robotis +#endif //TURTLEBOT3_NODE_ODOMETRY_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/battery_state.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/battery_state.hpp new file mode 100644 index 00000000..88090898 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/battery_state.hpp @@ -0,0 +1,49 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_SENSORS_BATTERY_STATE_HPP_ +#define TURTLEBOT3_NODE_SENSORS_BATTERY_STATE_HPP_ + +#include + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class BatteryState : public Sensors +{ + public: + explicit BatteryState( + std::shared_ptr & nh, + const std::string & topic_name = "battery_state"); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + + private: + rclcpp::Publisher::SharedPtr pub_; +}; +} // sensors +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_SENSORS_BATTERY_STATE_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/imu.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/imu.hpp new file mode 100644 index 00000000..a2c7d2ae --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/imu.hpp @@ -0,0 +1,53 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_SENSORS_IMU_HPP_ +#define TURTLEBOT3_NODE_SENSORS_IMU_HPP_ + +#include +#include + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class Imu : public Sensors +{ + public: + explicit Imu( + std::shared_ptr & nh, + const std::string & imu_topic_name = "imu", + const std::string & mag_topic_name = "magnetic_field", + const std::string & frame_id = "imu_link"); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + + private: + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr mag_pub_; +}; +} // sensors +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_SENSORS_IMU_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/joint_state.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/joint_state.hpp new file mode 100644 index 00000000..a3a2c03c --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/joint_state.hpp @@ -0,0 +1,58 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_SENSORS_JOINT_STATE_HPP_ +#define TURTLEBOT3_NODE_SENSORS_JOINT_STATE_HPP_ + +#include + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +constexpr uint8_t JOINT_NUM = 2; + +// ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#goal-velocity104 +constexpr double RPM_TO_MS = 0.229 * 0.0034557519189487725; + +// 0.087890625[deg] * 3.14159265359 / 180 = 0.001533981f +constexpr double TICK_TO_RAD = 0.001533981; + +class JointState : public Sensors +{ + public: + explicit JointState( + std::shared_ptr & nh, + const std::string & topic_name = "joint_states", + const std::string & frame_id = "base_link"); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + + private: + rclcpp::Publisher::SharedPtr pub_; +}; +} // sensors +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_SENSORS_JOINT_STATE_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/sensor_state.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/sensor_state.hpp new file mode 100644 index 00000000..229e9306 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/sensor_state.hpp @@ -0,0 +1,60 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_SENSORS_SENSOR_STATE_HPP_ +#define TURTLEBOT3_NODE_SENSORS_SENSOR_STATE_HPP_ + +#include + +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +namespace sensors +{ +class SensorState : public Sensors +{ + public: + explicit SensorState( + std::shared_ptr & nh, + const std::string & topic_name = "sensor_state", + const bool & bumper_forward = false, + const bool & bumper_backward = false, + const bool & cliff = false, + const bool & sonar = false, + const bool & illumination = false); + + void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) override; + + private: + rclcpp::Publisher::SharedPtr pub_; + + bool bumper_forward_; + bool bumper_backward_; + bool cliff_; + bool sonar_; + bool illumination_; +}; +} // sensors +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_SENSORS_SENSOR_STATE_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/sensors/sensors.hpp b/turtlebot3_node/include/turtlebot3_node/sensors/sensors.hpp new file mode 100644 index 00000000..d1bd9c4d --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/sensors/sensors.hpp @@ -0,0 +1,61 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_SENSORS_SENSORS_HPP_ +#define TURTLEBOT3_NODE_SENSORS_SENSORS_HPP_ + +#include +#include +#include + +#include + +#include "turtlebot3_node/control_table.hpp" +#include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +extern const ControlTable extern_control_table; +namespace sensors +{ +class Sensors +{ + public: + explicit Sensors( + std::shared_ptr & nh, + const std::string & frame_id = "") + : nh_(nh), + frame_id_(frame_id) + { + } + + virtual void publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) = 0; + + protected: + std::shared_ptr nh_; + std::string frame_id_; + rclcpp::QoS qos_ = rclcpp::QoS(rclcpp::KeepLast(10)); +}; +} // sensors +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_SENSORS_SENSORS_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp new file mode 100644 index 00000000..efefafb3 --- /dev/null +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -0,0 +1,119 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#ifndef TURTLEBOT3_NODE_TURTLEBOT3_HPP_ +#define TURTLEBOT3_NODE_TURTLEBOT3_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "turtlebot3_node/control_table.hpp" +#include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" +#include "turtlebot3_node/odometry.hpp" + +#include "turtlebot3_node/devices/devices.hpp" +#include "turtlebot3_node/devices/motor_power.hpp" +#include "turtlebot3_node/devices/reset.hpp" +#include "turtlebot3_node/devices/sound.hpp" + +#include "turtlebot3_node/sensors/battery_state.hpp" +#include "turtlebot3_node/sensors/imu.hpp" +#include "turtlebot3_node/sensors/joint_state.hpp" +#include "turtlebot3_node/sensors/sensor_state.hpp" +#include "turtlebot3_node/sensors/sensors.hpp" + +namespace robotis +{ +namespace turtlebot3 +{ +extern const ControlTable extern_control_table; +class TurtleBot3 : public rclcpp::Node +{ + public: + typedef struct + { + float separation; + float radius; + } Wheels; + + typedef struct + { + float profile_acceleration_constant; + float profile_acceleration; + } Motors; + + explicit TurtleBot3(const std::string & usb_port); + virtual ~TurtleBot3(){}; + + Wheels * get_wheels(); + Motors * get_motors(); + + private: + void init_dynamixel_sdk_wrapper(const std::string & usb_port); + void check_device_status(); + + void add_sensors(); + void add_devices(); + void add_motors(); + void add_wheels(); + + void run(); + + void publish_timer(const std::chrono::milliseconds timeout); + void heartbeat_timer(const std::chrono::milliseconds timeout); + + void cmd_vel_callback(); + void parameter_event_callback(); + + Wheels wheels_; + Motors motors_; + + std::shared_ptr dxl_sdk_wrapper_; + + std::list sensors_; + std::map devices_; + + std::unique_ptr odom_; + + rclcpp::Node::SharedPtr node_handle_; + + rclcpp::TimerBase::SharedPtr publish_timer_; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + + rclcpp::AsyncParametersClient::SharedPtr priv_parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; +}; +} // turtlebot3 +} // robotis +#endif // TURTLEBOT3_NODE_TURTLEBOT3_HPP_ diff --git a/turtlebot3_node/package.xml b/turtlebot3_node/package.xml index f3dc11ad..8b653343 100644 --- a/turtlebot3_node/package.xml +++ b/turtlebot3_node/package.xml @@ -1,28 +1,32 @@ - + + turtlebot3_node - 0.0.1 + 2.0.0 - Include odometry and tf node + TurtleBot3 driver node that include diff drive controller, odometry and tf node Apache 2.0 Darby Lim - Pyo + Pyo Pyo - http://wiki.ros.org/turtlebot3_description + http://wiki.ros.org/turtlebot3_node http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues ament_cmake - rclcpp - std_msgs - sensor_msgs + dynamixel_sdk geometry_msgs + message_filters nav_msgs - turtlebot3_msgs - tf2 + rclcpp + rcutils + sensor_msgs + std_msgs + std_srvs tf2_ros - builtin_interfaces + tf2 + turtlebot3_msgs ament_cmake diff --git a/turtlebot3_node/param/burger.yaml b/turtlebot3_node/param/burger.yaml new file mode 100644 index 00000000..0d6445d7 --- /dev/null +++ b/turtlebot3_node/param/burger.yaml @@ -0,0 +1,37 @@ +turtlebot3_node: + ros__parameters: + + opencr: + id: 200 + baud_rate: 1000000 + protocol_version: 2.0 + + wheels: + separation: 0.160 + radius: 0.033 + + motors: + profile_acceleration_constant: 214.577 + + # [rev/min2] + # ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#profile-acceleration + profile_acceleration: 0.0 + + sensors: + bumper_1: false + bumper_2: false + + illumination: false + + ir: false + + sonar: false + +diff_drive_controller: + ros__parameters: + + odometry: + publish_tf: true + use_imu: true + frame_id: "odom" + child_frame_id: "base_footprint" diff --git a/turtlebot3_node/param/waffle.yaml b/turtlebot3_node/param/waffle.yaml new file mode 100644 index 00000000..4693b835 --- /dev/null +++ b/turtlebot3_node/param/waffle.yaml @@ -0,0 +1,37 @@ +turtlebot3_node: + ros__parameters: + + opencr: + id: 200 + baud_rate: 1000000 + protocol_version: 2.0 + + wheels: + separation: 0.287 + radius: 0.033 + + motors: + profile_acceleration_constant: 214.577 + + # [rev/min2] + # ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#profile-acceleration + profile_acceleration: 0.0 + + sensors: + bumper_1: false + bumper_2: false + + illumination: false + + ir: false + + sonar: false + +diff_drive_controller: + ros__parameters: + + odometry: + publish_tf: true + use_imu: true + frame_id: "odom" + child_frame_id: "base_footprint" diff --git a/turtlebot3_node/param/waffle_pi.yaml b/turtlebot3_node/param/waffle_pi.yaml new file mode 100644 index 00000000..4693b835 --- /dev/null +++ b/turtlebot3_node/param/waffle_pi.yaml @@ -0,0 +1,37 @@ +turtlebot3_node: + ros__parameters: + + opencr: + id: 200 + baud_rate: 1000000 + protocol_version: 2.0 + + wheels: + separation: 0.287 + radius: 0.033 + + motors: + profile_acceleration_constant: 214.577 + + # [rev/min2] + # ref) http://emanual.robotis.com/docs/en/dxl/x/xl430-w250/#profile-acceleration + profile_acceleration: 0.0 + + sensors: + bumper_1: false + bumper_2: false + + illumination: false + + ir: false + + sonar: false + +diff_drive_controller: + ros__parameters: + + odometry: + publish_tf: true + use_imu: true + frame_id: "odom" + child_frame_id: "base_footprint" diff --git a/turtlebot3_node/src/devices/motor_power.cpp b/turtlebot3_node/src/devices/motor_power.cpp new file mode 100644 index 00000000..eeaa128b --- /dev/null +++ b/turtlebot3_node/src/devices/motor_power.cpp @@ -0,0 +1,60 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/devices/motor_power.hpp" + +using namespace robotis; +using namespace turtlebot3; + +devices::MotorPower::MotorPower( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper, + const std::string & server_name) +: Devices(nh, dxl_sdk_wrapper) +{ + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create motor power server"); + srv_ = nh_->create_service( + server_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) -> void + { + this->command(static_cast(request.get()), static_cast(response.get())); + } + ); +} + +void devices::MotorPower::command(const void * request, void * response) +{ + std_srvs::srv::SetBool::Request req = *(std_srvs::srv::SetBool::Request*)request; + std_srvs::srv::SetBool::Response *res = (std_srvs::srv::SetBool::Response*)response; + + res->success = dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.motor_torque_enable.addr, + extern_control_table.motor_torque_enable.length, + (uint8_t*)&req.data, + &res->message); +} + +void devices::MotorPower::request( + rclcpp::Client::SharedPtr client, + std_srvs::srv::SetBool::Request req) +{ + auto request = std::make_shared(req); + auto result = client->async_send_request(request); +} diff --git a/turtlebot3_node/src/devices/reset.cpp b/turtlebot3_node/src/devices/reset.cpp new file mode 100644 index 00000000..ddb0b5c5 --- /dev/null +++ b/turtlebot3_node/src/devices/reset.cpp @@ -0,0 +1,67 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/devices/reset.hpp" + +using namespace robotis; +using namespace turtlebot3; + +devices::Reset::Reset( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper, + const std::string & server_name) +: Devices(nh, dxl_sdk_wrapper) +{ + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create reset server"); + srv_ = nh_->create_service( + server_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) -> void + { + this->command(static_cast(request.get()), static_cast(response.get())); + } + ); +} + +void devices::Reset::command(const void * request, void * response) +{ + (void) request; + + std_srvs::srv::Trigger::Response *res = (std_srvs::srv::Trigger::Response*)response; + + uint8_t reset = 1; + + res->success = dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.imu_re_calibration.addr, + extern_control_table.imu_re_calibration.length, + &reset, + &res->message); + + RCLCPP_INFO(nh_->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(nh_->get_logger(), "Calibration End"); +} + +void devices::Reset::request( + rclcpp::Client::SharedPtr client, + std_srvs::srv::Trigger::Request req) +{ + auto request = std::make_shared(req); + auto result = client->async_send_request(request); +} diff --git a/turtlebot3_node/src/devices/sound.cpp b/turtlebot3_node/src/devices/sound.cpp new file mode 100644 index 00000000..b50139e1 --- /dev/null +++ b/turtlebot3_node/src/devices/sound.cpp @@ -0,0 +1,60 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/devices/sound.hpp" + +using namespace robotis; +using namespace turtlebot3; + +devices::Sound::Sound( + std::shared_ptr & nh, + std::shared_ptr & dxl_sdk_wrapper, + const std::string & server_name) +: Devices(nh, dxl_sdk_wrapper) +{ + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create sound server"); + srv_ = nh_->create_service( + server_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) -> void + { + this->command(static_cast(request.get()), static_cast(response.get())); + } + ); +} + +void devices::Sound::command(const void * request, void * response) +{ + turtlebot3_msgs::srv::Sound::Request req = *(turtlebot3_msgs::srv::Sound::Request*)request; + turtlebot3_msgs::srv::Sound::Response *res = (turtlebot3_msgs::srv::Sound::Response*)response; + + res->success = dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.sound.addr, + extern_control_table.sound.length, + (uint8_t*)&req.value, + &res->message); +} + +void devices::Sound::request( + rclcpp::Client::SharedPtr client, + turtlebot3_msgs::srv::Sound::Request req) +{ + auto request = std::make_shared(req); + auto result = client->async_send_request(request); +} diff --git a/turtlebot3_node/src/diff_drive_controller.cpp b/turtlebot3_node/src/diff_drive_controller.cpp new file mode 100644 index 00000000..8a6743fc --- /dev/null +++ b/turtlebot3_node/src/diff_drive_controller.cpp @@ -0,0 +1,35 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/diff_drive_controller.hpp" + +using namespace robotis; +using namespace turtlebot3; + +DiffDriveController::DiffDriveController(const float wheel_seperation, const float wheel_radius) +: Node("diff_drive_controller", rclcpp::NodeOptions().use_intra_process_comms(true)) +{ + nh_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); + + odometry_ = std::make_unique( + nh_, + wheel_seperation, + wheel_radius); + + RCLCPP_INFO(this->get_logger(), "Run!"); +} diff --git a/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp b/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp new file mode 100644 index 00000000..ca261986 --- /dev/null +++ b/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp @@ -0,0 +1,205 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" + +using namespace robotis; +using namespace turtlebot3; + +DynamixelSDKWrapper::DynamixelSDKWrapper(const Device & device) : device_(device) +{ + if (init_dynamixel_sdk_handlers() == false) + { + LOG_ERROR("DynamixelSDKWrapper", "Failed to initialize SDK handlers"); + return; + } + else + { + LOG_DEBUG("DynamixelSDKWrapper", "Success to initilize SDK handlers"); + } +} + +DynamixelSDKWrapper::~DynamixelSDKWrapper() +{ + portHandler_->closePort(); +} + +bool DynamixelSDKWrapper::is_connected_to_device() +{ + uint8_t data[2]; + return this->read_register(device_.id, 0, 2, &data[0]); +} + +void DynamixelSDKWrapper::init_read_memory(const uint16_t & start_addr, const uint16_t & length) +{ + read_memory_.start_addr = start_addr; + read_memory_.length = length; + read_memory_.data = &read_data_[0]; +} + +void DynamixelSDKWrapper::read_data_set() +{ + const char *log = NULL; + bool ret = this->read_register( + device_.id, + read_memory_.start_addr, + read_memory_.length, + &read_data_buffer_[0], + &log); + + if (ret == false) + { + LOG_ERROR("DynamixelSDKWrapper", "Failed to read[%s]", log); + } + else + { + std::lock_guard lock(read_data_mutex_); + std::copy(read_data_buffer_, read_data_buffer_ + READ_DATA_SIZE, read_data_); + LOG_DEBUG("DynamixelSDKWrapper", "Succeeded to read"); + } +} + +bool DynamixelSDKWrapper::set_data_to_device( + const uint16_t & addr, + const uint16_t & length, + uint8_t * get_data, + std::string * msg) +{ + const char *log = nullptr; + bool ret = false; + + std::lock_guard lock(write_data_mutex_); + ret = write_register(device_.id, addr, length, get_data, &log); + + if (ret == true) + { + *msg = "Succeeded to write data"; + return true; + } + else + { + *msg = "Failed to write data" + std::string(log); + return false; + } + + return false; +} + +bool DynamixelSDKWrapper::init_dynamixel_sdk_handlers() +{ + portHandler_ = dynamixel::PortHandler::getPortHandler(device_.usb_port.c_str()); + packetHandler_ = dynamixel::PacketHandler::getPacketHandler((int)device_.protocol_version); + + if (portHandler_->openPort()) + { + LOG_INFO("DynamixelSDKWrapper", "Succeeded to open the port(%s)!", device_.usb_port.c_str()); + } + else + { + LOG_ERROR("DynamixelSDKWrapper", "Failed to open the port(%s)!", device_.usb_port.c_str()); + return false; + } + + if (portHandler_->setBaudRate((int)device_.baud_rate)) + { + LOG_INFO("DynamixelSDKWrapper", "Succeeded to change the baudrate!"); + } + else + { + LOG_ERROR("DynamixelSDKWrapper", "Failed to change the baudrate(%d)!", device_.baud_rate); + return false; + } + + return true; +} + +bool DynamixelSDKWrapper::read_register( + uint8_t id, + uint16_t address, + uint16_t length, + uint8_t * data_basket, + const char ** log) +{ + std::lock_guard lock(sdk_mutex_); + + int32_t dxl_comm_result = COMM_RX_FAIL; + uint8_t dxl_error = 0; + + dxl_comm_result = packetHandler_->readTxRx( + portHandler_, + id, + address, + length, + data_basket, + &dxl_error); + + if (dxl_comm_result != COMM_SUCCESS) + { + if (log != NULL) *log = packetHandler_->getTxRxResult(dxl_comm_result); + return false; + } + else if (dxl_error != 0) + { + if (log != NULL) *log = packetHandler_->getRxPacketError(dxl_error); + return false; + } + else + { + return true; + } + + return false; +} + +bool DynamixelSDKWrapper::write_register( + uint8_t id, + uint16_t address, + uint16_t length, + uint8_t * data, + const char ** log) +{ + std::lock_guard lock(sdk_mutex_); + + int32_t dxl_comm_result = COMM_TX_FAIL; + uint8_t dxl_error = 0; + + dxl_comm_result = packetHandler_->writeTxRx( + portHandler_, + id, + address, + length, + data, + &dxl_error); + + if (dxl_comm_result != COMM_SUCCESS) + { + if (log != NULL) *log = packetHandler_->getTxRxResult(dxl_comm_result); + return false; + } + else if (dxl_error != 0) + { + if (log != NULL) *log = packetHandler_->getRxPacketError(dxl_error); + return false; + } + else + { + return true; + } + + return false; +} diff --git a/turtlebot3_node/src/joint_states.cpp b/turtlebot3_node/src/joint_states.cpp deleted file mode 100644 index 7c8d8b4c..00000000 --- a/turtlebot3_node/src/joint_states.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ - -/* Author: Darby Lim */ - -#include "joint_state.h" - -using namespace turtlebot3; - -constexpr double DXL_TICK2RAD = (0.087890625f * 3.14159265359f) / 180.0f; -constexpr char FRAME_ID_OF_JOINT_STATE[] = "base_link"; -constexpr char LEFT_WHEEL_JOINT_NAME[] = "wheel_left_joint"; -constexpr char RIGHT_WHEEL_JOINT_NAME[] = "wheel_right_joint"; - -sensor_msgs::msg::JointState JointState::getJointState(const rclcpp::Time now) -{ - static rclcpp::Time last_time = now; - rclcpp::Duration duration(now.nanoseconds() - last_time.nanoseconds()); - - sensor_msgs::msg::JointState joint_state; - - joint_state.header.frame_id = FRAME_ID_OF_JOINT_STATE; - joint_state.header.stamp = now; - - joint_state.name.push_back(LEFT_WHEEL_JOINT_NAME); - joint_state.name.push_back(RIGHT_WHEEL_JOINT_NAME); - - std::lock_guard lock(mutex_); - joint_state.position.push_back(last_rad_[0]); - joint_state.position.push_back(last_rad_[1]); - - if (duration > rclcpp::Duration(0,0)) // can't have negative or zero duration - { - joint_state.velocity.push_back(DXL_TICK2RAD*last_diff_tick_[0]/(duration.seconds())); - joint_state.velocity.push_back(DXL_TICK2RAD*last_diff_tick_[1]/(duration.seconds())); - } - - joint_state.effort.push_back(0.0f); - joint_state.effort.push_back(0.0f); - - last_time = now; - return joint_state; -} - -void JointState::updateRadianFromTick(const turtlebot3_msgs::msg::SensorState::SharedPtr state) -{ - std::array current_tick = {state->left_encoder, state->right_encoder}; - static std::array last_tick = current_tick; - - for (uint8_t index = 0; index < current_tick.size(); index++) - { - std::lock_guard lock(mutex_); - last_diff_tick_[index] = current_tick[index] - last_tick[index]; - last_rad_[index] += (DXL_TICK2RAD * static_cast(last_diff_tick_[index])); - last_tick[index] = current_tick[index]; - } -} diff --git a/turtlebot3_node/src/node_main.cpp b/turtlebot3_node/src/node_main.cpp index 0e77e7eb..de1c8932 100644 --- a/turtlebot3_node/src/node_main.cpp +++ b/turtlebot3_node/src/node_main.cpp @@ -20,118 +20,54 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include +#include -#include "turtlebot3_msgs/msg/sensor_state.hpp" -#include "sensor_msgs/msg/joint_state.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "builtin_interfaces/msg/time.hpp" +#include "turtlebot3_node/diff_drive_controller.hpp" +#include "turtlebot3_node/turtlebot3.hpp" -#include - -#include "joint_state.h" -#include "odometry.h" - -using namespace std::chrono_literals; - -namespace turtlebot3 +void help_print() { -class TurtleBot3 : public rclcpp::Node -{ - public: - explicit TurtleBot3() - : Node("turtlebot3_node") - { - RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); - - node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); - - joint_state_ = std::make_unique(); - odom_ = std::make_unique(); - - tf_broadcaster_ = std::make_shared(node_handle_); - - joint_state_pub_ = this->create_publisher("joint_states", 10); - odom_pub_ = this->create_publisher("odom", 10); - time_pub_ = this->create_publisher("time_sync", 10); - - sensor_state_sub_ = this->create_subscription( - "sensor_state", - 10, - [this](const turtlebot3_msgs::msg::SensorState::SharedPtr sensor_state) -> void - { - joint_state_->updateRadianFromTick(sensor_state); - } - ); - - imu_sub_ = this->create_subscription( - "imu", - 10, - [this](const sensor_msgs::msg::Imu::SharedPtr imu) -> void - { - odom_->updateImu(imu); - } - ); - - odom_timer_ = this->create_wall_timer( - 33ms, - [this]() - { - constexpr double WheelRadius = 0.033f; - - sensor_msgs::msg::JointState joint_state_msg = joint_state_->getJointState(now()); - - odom_->updateJointState(joint_state_msg); - - odom_pub_->publish(odom_->getOdom(now(), WheelRadius)); - tf_broadcaster_->sendTransform(odom_->getOdomTf()); + printf("For turtlebot3 node : \n"); + printf("turtlebot3_node [-i usb_port] [-h]\n"); + printf("options:\n"); + printf("-h : Print this help function.\n"); + printf("-i usb_port: Connected USB port with OpenCR."); +} - joint_state_pub_->publish(joint_state_msg); - } - ); +int main(int argc, char *argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); - time_timer_ = this->create_wall_timer( - 1s, - [this]() - { - auto time_msg = builtin_interfaces::msg::Time(); - time_msg = now(); - time_pub_->publish(time_msg); - } - ); + if (rcutils_cli_option_exist(argv, argv + argc, "-h")) + { + help_print(); + return 0; } - virtual ~TurtleBot3(){}; - - private: - rclcpp::Node::SharedPtr node_handle_; - - std::unique_ptr joint_state_; - std::unique_ptr odom_; - - std::shared_ptr tf_broadcaster_; - - rclcpp::Subscription::SharedPtr sensor_state_sub_; - rclcpp::Subscription::SharedPtr imu_sub_; - - rclcpp::Publisher::SharedPtr joint_state_pub_; - rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr time_pub_; + rclcpp::init(argc, argv); - rclcpp::TimerBase::SharedPtr odom_timer_; - rclcpp::TimerBase::SharedPtr time_timer_; -}; -} + std::string usb_port = "/dev/ttyACM0"; + char * cli_options; + cli_options = rcutils_cli_get_option(argv, argv + argc, "-i"); + if (nullptr != cli_options) + { + usb_port = std::string(cli_options); + } -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); + auto turtlebot3 = std::make_shared(usb_port); + auto diff_drive_controller = + std::make_shared( + turtlebot3->get_wheels()->separation, + turtlebot3->get_wheels()->radius); - rclcpp::spin(node); + executor.add_node(turtlebot3); + executor.add_node(diff_drive_controller); + executor.spin(); rclcpp::shutdown(); + return 0; -} \ No newline at end of file +} diff --git a/turtlebot3_node/src/odometry.cpp b/turtlebot3_node/src/odometry.cpp index 39dc0aaa..898cb59f 100644 --- a/turtlebot3_node/src/odometry.cpp +++ b/turtlebot3_node/src/odometry.cpp @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright 2017 ROBOTIS CO., LTD. +* Copyright 2019 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. @@ -16,132 +16,260 @@ /* Author: Darby Lim */ -#include "odometry.h" -#include +#include "turtlebot3_node/odometry.hpp" +using namespace robotis; +using namespace std::chrono_literals; using namespace turtlebot3; -constexpr char FRAME_ID_OF_ODOMETRY[] = "odom"; -constexpr char CHILD_FRAME_ID_OF_ODOMETRY[] = "base_footprint"; -nav_msgs::msg::Odometry Odometry::getOdom(const rclcpp::Time now, const double wheel_radius) +Odometry::Odometry( + std::shared_ptr &nh, + const double wheels_separation, + const double wheels_radius) +: nh_(nh), + wheels_separation_(wheels_separation), + wheels_radius_(wheels_radius), + use_imu_(false), + publish_tf_(false), + imu_angle_(0.0f) { - static rclcpp::Time last_time = now; - rclcpp::Duration duration(now.nanoseconds() - last_time.nanoseconds()); - - calcOdometry(duration, wheel_radius); - - auto odom = nav_msgs::msg::Odometry(); + RCLCPP_INFO(nh_->get_logger(), "Init Odometry"); + + nh_->declare_parameter("odometry.frame_id"); + nh_->declare_parameter("odometry.child_frame_id"); + + nh_->declare_parameter("odometry.use_imu"); + nh_->declare_parameter("odometry.publish_tf"); + + nh_->get_parameter_or( + "odometry.use_imu", + use_imu_, + false); + + nh_->get_parameter_or( + "odometry.publish_tf", + publish_tf_, + false); + + nh_->get_parameter_or( + "odometry.frame_id", + frame_id_of_odometry_, + std::string("odom")); + + nh_->get_parameter_or( + "odometry.child_frame_id", + child_frame_id_of_odometry_, + std::string("base_footprint")); + + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + odom_pub_ = nh_->create_publisher("odom", qos); + + tf_broadcaster_ = std::make_unique(nh_); + + if (use_imu_) + { + uint32_t queue_size = 10; + joint_state_imu_sync_ = std::make_shared(queue_size); + + msg_ftr_joint_state_sub_ = + std::make_shared>( + nh_, + "joint_states"); + + msg_ftr_imu_sub_ = + std::make_shared>( + nh_, + "imu"); + + // connect message filters to synchronizer + joint_state_imu_sync_->connectInput(*msg_ftr_joint_state_sub_, *msg_ftr_imu_sub_); + + joint_state_imu_sync_->setInterMessageLowerBound( + 0, + rclcpp::Duration(75ms)); + + joint_state_imu_sync_->setInterMessageLowerBound( + 1, + rclcpp::Duration(15ms)); + + joint_state_imu_sync_->registerCallback(std::bind( + &Odometry::joint_state_and_imu_callback, + this, + std::placeholders::_1, + std::placeholders::_2)); + } + else + { + joint_state_sub_ = nh_->create_subscription( + "joint_states", + qos, + std::bind(&Odometry::joint_state_callback, this, std::placeholders::_1)); + } +} - odom.header.frame_id = FRAME_ID_OF_ODOMETRY; - odom.child_frame_id = CHILD_FRAME_ID_OF_ODOMETRY; - odom.header.stamp = now; +void Odometry::joint_state_callback(const sensor_msgs::msg::JointState::SharedPtr joint_state_msg) +{ + static rclcpp::Time last_time = joint_state_msg->header.stamp; + rclcpp::Duration duration(joint_state_msg->header.stamp.nanosec - last_time.nanoseconds()); - odom.pose.pose.position.x = odom_pose_[0]; - odom.pose.pose.position.y = odom_pose_[1]; - odom.pose.pose.position.z = 0; + update_joint_state(joint_state_msg); + calculate_odometry(duration); + publish(joint_state_msg->header.stamp); - tf2::Quaternion q; - q.setRPY(0.0, 0.0, odom_pose_[2]); - odom.pose.pose.orientation.x = q.x(); - odom.pose.pose.orientation.y = q.y(); - odom.pose.pose.orientation.z = q.z(); - odom.pose.pose.orientation.w = q.w(); - - odom.twist.twist.linear.x = odom_vel_[0]; - odom.twist.twist.angular.z = odom_vel_[2]; - - updateOdomTf(now, odom); - last_time = now; - return odom; + last_time = joint_state_msg->header.stamp; } -const geometry_msgs::msg::TransformStamped Odometry::getOdomTf() +void Odometry::joint_state_and_imu_callback( + const std::shared_ptr &joint_state_msg, + const std::shared_ptr &imu_msg) { - std::lock_guard lock(tf_mutex_); - return odom_tf_; + RCLCPP_DEBUG( + nh_->get_logger(), + "[joint_state_msg_] nanosec : %d [imu_msg] nanosec : %d", + joint_state_msg->header.stamp.nanosec, + imu_msg->header.stamp.nanosec); + + static rclcpp::Time last_time = joint_state_msg->header.stamp; + rclcpp::Duration duration(joint_state_msg->header.stamp.nanosec - last_time.nanoseconds()); + + update_joint_state(joint_state_msg); + update_imu(imu_msg); + calculate_odometry(duration); + publish(joint_state_msg->header.stamp); + + last_time = joint_state_msg->header.stamp; } -void Odometry::updateOdomTf(const rclcpp::Time now, const nav_msgs::msg::Odometry odom) +void Odometry::publish(const rclcpp::Time & now) { - std::lock_guard lock(tf_mutex_); + auto odom_msg = std::make_unique(); + + odom_msg->header.frame_id = frame_id_of_odometry_; + odom_msg->child_frame_id = child_frame_id_of_odometry_; + odom_msg->header.stamp = now; - odom_tf_.transform.translation.x = odom.pose.pose.position.x; - odom_tf_.transform.translation.y = odom.pose.pose.position.y; - odom_tf_.transform.translation.z = odom.pose.pose.position.z; - odom_tf_.transform.rotation = odom.pose.pose.orientation; + odom_msg->pose.pose.position.x = robot_pose_[0]; + odom_msg->pose.pose.position.y = robot_pose_[1]; + odom_msg->pose.pose.position.z = 0; - odom_tf_.header.frame_id = FRAME_ID_OF_ODOMETRY; - odom_tf_.child_frame_id = CHILD_FRAME_ID_OF_ODOMETRY; - odom_tf_.header.stamp = now; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, robot_pose_[2]); + + odom_msg->pose.pose.orientation.x = q.x(); + odom_msg->pose.pose.orientation.y = q.y(); + odom_msg->pose.pose.orientation.z = q.z(); + odom_msg->pose.pose.orientation.w = q.w(); + + odom_msg->twist.twist.linear.x = robot_vel_[0]; + odom_msg->twist.twist.angular.z = robot_vel_[2]; + + // TODO: Find more accurate covariance. + // odom_msg->pose.covariance[0] = 0.05; + // odom_msg->pose.covariance[7] = 0.05; + // odom_msg->pose.covariance[14] = 1.0e-9; + // odom_msg->pose.covariance[21] = 1.0e-9; + // odom_msg->pose.covariance[28] = 1.0e-9; + // odom_msg->pose.covariance[35] = 0.0872665; + + // odom_msg->twist.covariance[0] = 0.001; + // odom_msg->twist.covariance[7] = 1.0e-9; + // odom_msg->twist.covariance[14] = 1.0e-9; + // odom_msg->twist.covariance[21] = 1.0e-9; + // odom_msg->twist.covariance[28] = 1.0e-9; + // odom_msg->twist.covariance[35] = 0.001; + + geometry_msgs::msg::TransformStamped odom_tf; + + odom_tf.transform.translation.x = odom_msg->pose.pose.position.x; + odom_tf.transform.translation.y = odom_msg->pose.pose.position.y; + odom_tf.transform.translation.z = odom_msg->pose.pose.position.z; + odom_tf.transform.rotation = odom_msg->pose.pose.orientation; + + odom_tf.header.frame_id = frame_id_of_odometry_; + odom_tf.child_frame_id = child_frame_id_of_odometry_; + odom_tf.header.stamp = now; + + odom_pub_->publish(std::move(odom_msg)); + + if (publish_tf_) + tf_broadcaster_->sendTransform(odom_tf); } -void Odometry::updateJointState(const sensor_msgs::msg::JointState& joint_state) +void Odometry::update_joint_state( + const std::shared_ptr &joint_state) { - std::lock_guard lock(robot_mutex_); - static double last_joint_positions[2] = {0.0f, 0.0f}; + static std::array last_joint_positions = {0.0f, 0.0f}; - diff_mobile_.diff_wheels[0] = joint_state.position[0] - last_joint_positions[0]; - diff_mobile_.diff_wheels[1] = joint_state.position[1] - last_joint_positions[1]; + diff_joint_positions_[0] = joint_state->position[0] - last_joint_positions[0]; + diff_joint_positions_[1] = joint_state->position[1] - last_joint_positions[1]; - last_joint_positions[0] = joint_state.position[0]; - last_joint_positions[1] = joint_state.position[1]; + last_joint_positions[0] = joint_state->position[0]; + last_joint_positions[1] = joint_state->position[1]; } -void Odometry::updateImu(const sensor_msgs::msg::Imu::SharedPtr imu) +void Odometry::update_imu(const std::shared_ptr &imu) { - std::lock_guard lock(robot_mutex_); - diff_mobile_.theta = atan2f(imu->orientation.x*imu->orientation.y + imu->orientation.w*imu->orientation.z, - 0.5f - imu->orientation.y*imu->orientation.y - imu->orientation.z*imu->orientation.z); + imu_angle_ = atan2f( + imu->orientation.x*imu->orientation.y + imu->orientation.w*imu->orientation.z, + 0.5f - imu->orientation.y*imu->orientation.y - imu->orientation.z*imu->orientation.z); } -bool Odometry::calcOdometry(const rclcpp::Duration duration, const double wheel_radius) +bool Odometry::calculate_odometry(const rclcpp::Duration &duration) { - std::lock_guard lock(robot_mutex_); + // rotation value of wheel [rad] + double wheel_l = diff_joint_positions_[0]; + double wheel_r = diff_joint_positions_[1]; - double wheel_l = 0.0f; - double wheel_r = 0.0f; // rotation value of wheel [rad] + double delta_s = 0.0; + double delta_theta = 0.0; - double delta_s = 0.0f; - double delta_theta = 0.0f; + double theta = 0.0; + static double last_theta = 0.0; - double theta = 0.0f; - static double last_theta = 0.0f; + // v = translational velocity [m/s] + // w = rotational velocity [rad/s] + double v = 0.0; + double w = 0.0; - double v = 0.0f; // v = translational velocity [m/s] - double w = 0.0f; // w = rotational velocity [rad/s] - double step_time = duration.seconds(); - if (step_time == 0.0f) + if (step_time == 0.0) return false; - wheel_l = diff_mobile_.diff_wheels[0]; - wheel_r = diff_mobile_.diff_wheels[1]; - if (std::isnan(wheel_l)) - wheel_l = 0.0f; + wheel_l = 0.0; if (std::isnan(wheel_r)) - wheel_r = 0.0f; + wheel_r = 0.0; - delta_s = wheel_radius * (wheel_r + wheel_l) / 2.0f; - theta = diff_mobile_.theta; + delta_s = wheels_radius_ * (wheel_r + wheel_l) / 2.0; - delta_theta = theta - last_theta; + if (use_imu_) + { + theta = imu_angle_; + delta_theta = theta - last_theta; + } + else + { + theta = wheels_radius_ * (wheel_r - wheel_l) / wheels_separation_; + delta_theta = theta; + } // compute odometric pose - odom_pose_[0] += delta_s * cos(odom_pose_[2] + (delta_theta / 2.0f)); - odom_pose_[1] += delta_s * sin(odom_pose_[2] + (delta_theta / 2.0f)); - odom_pose_[2] += delta_theta; + robot_pose_[0] += delta_s * cos(robot_pose_[2] + (delta_theta / 2.0)); + robot_pose_[1] += delta_s * sin(robot_pose_[2] + (delta_theta / 2.0)); + robot_pose_[2] += delta_theta; + + RCLCPP_DEBUG(nh_->get_logger(), "x : %f, y : %f", robot_pose_[0], robot_pose_[1]); // compute odometric instantaneouse velocity v = delta_s / step_time; w = delta_theta / step_time; - odom_vel_[0] = v; - odom_vel_[1] = 0.0; - odom_vel_[2] = w; + robot_vel_[0] = v; + robot_vel_[1] = 0.0; + robot_vel_[2] = w; last_theta = theta; return true; -} \ No newline at end of file +} diff --git a/turtlebot3_node/src/odometry.h b/turtlebot3_node/src/odometry.h deleted file mode 100644 index 3f7f73e8..00000000 --- a/turtlebot3_node/src/odometry.h +++ /dev/null @@ -1,70 +0,0 @@ -/******************************************************************************* -* Copyright 2019 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. -*******************************************************************************/ - -/* Author: Darby Lim */ - -#ifndef TURTLEBOT3_ODOMETRY_H -#define TURTLEBOT3_ODOMETRY_H - -#include -#include -#include - -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" - -#include "sensor_msgs/msg/joint_state.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" - -#include "tf2/LinearMath/Quaternion.h" - -namespace turtlebot3 -{ - -typedef struct robot -{ - std::array diff_wheels; - double theta; -}Robot; - -class Odometry -{ - public: - Odometry(){}; - virtual ~Odometry(){}; - - nav_msgs::msg::Odometry getOdom(const rclcpp::Time now, const double wheel_radius); - const geometry_msgs::msg::TransformStamped getOdomTf(); - void updateOdomTf(const rclcpp::Time now, const nav_msgs::msg::Odometry odom); - void updateImu(const sensor_msgs::msg::Imu::SharedPtr imu); - void updateJointState(const sensor_msgs::msg::JointState& joint_state); - - private: - bool calcOdometry(const rclcpp::Duration duration, const double wheel_radius); - - std::mutex robot_mutex_, tf_mutex_; - Robot diff_mobile_; - - geometry_msgs::msg::TransformStamped odom_tf_; - - std::array odom_pose_; - std::array odom_vel_; -}; -} - -#endif //TURTLEBOT3_ODOMETRY_H \ No newline at end of file diff --git a/turtlebot3_node/src/sensors/battery_state.cpp b/turtlebot3_node/src/sensors/battery_state.cpp new file mode 100644 index 00000000..07e1d091 --- /dev/null +++ b/turtlebot3_node/src/sensors/battery_state.cpp @@ -0,0 +1,55 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/sensors/battery_state.hpp" + +using namespace robotis; +using namespace turtlebot3; + +sensors::BatteryState::BatteryState( + std::shared_ptr & nh, + const std::string & topic_name) +: Sensors(nh) +{ + pub_ = nh->create_publisher(topic_name, this->qos_); + + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create battery state publisher"); +} + +void sensors::BatteryState::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + auto msg = std::make_unique(); + + msg->header.stamp = now; + + msg->design_capacity = 1.8f; + + msg->voltage = 0.01f * dxl_sdk_wrapper->get_data_from_device( + extern_control_table.battery_voltage.addr, + extern_control_table.battery_voltage.length); + + msg->percentage = 0.01f * dxl_sdk_wrapper->get_data_from_device( + extern_control_table.battery_percentage.addr, + extern_control_table.battery_percentage.length); + + msg->voltage <= 7.0 ? msg->present = false : msg->present = true; + + pub_->publish(std::move(msg)); +} diff --git a/turtlebot3_node/src/sensors/imu.cpp b/turtlebot3_node/src/sensors/imu.cpp new file mode 100644 index 00000000..9fca607b --- /dev/null +++ b/turtlebot3_node/src/sensors/imu.cpp @@ -0,0 +1,105 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/sensors/imu.hpp" + +using namespace robotis; +using namespace turtlebot3; + +sensors::Imu::Imu( + std::shared_ptr & nh, + const std::string & imu_topic_name, + const std::string & mag_topic_name, + const std::string & frame_id) +: Sensors(nh, frame_id) +{ + imu_pub_ = nh->create_publisher(imu_topic_name, this->qos_); + mag_pub_ = nh->create_publisher(mag_topic_name, this->qos_); + + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create imu publisher"); +} + +void sensors::Imu::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + auto imu_msg = std::make_unique(); + + imu_msg->header.frame_id = this->frame_id_; + imu_msg->header.stamp = now; + + imu_msg->orientation.w = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_orientation_w.addr, + extern_control_table.imu_orientation_w.length); + + imu_msg->orientation.x = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_orientation_x.addr, + extern_control_table.imu_orientation_x.length); + + imu_msg->orientation.y = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_orientation_y.addr, + extern_control_table.imu_orientation_y.length); + + imu_msg->orientation.z = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_orientation_z.addr, + extern_control_table.imu_orientation_z.length); + + imu_msg->angular_velocity.x = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_angular_velocity_x.addr, + extern_control_table.imu_angular_velocity_x.length); + + imu_msg->angular_velocity.y = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_angular_velocity_y.addr, + extern_control_table.imu_angular_velocity_y.length); + + imu_msg->angular_velocity.z = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_angular_velocity_z.addr, + extern_control_table.imu_angular_velocity_z.length); + + imu_msg->linear_acceleration.x = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_linear_acceleration_x.addr, + extern_control_table.imu_linear_acceleration_x.length); + + imu_msg->linear_acceleration.y = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_linear_acceleration_y.addr, + extern_control_table.imu_linear_acceleration_y.length); + + imu_msg->linear_acceleration.z = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_linear_acceleration_z.addr, + extern_control_table.imu_linear_acceleration_z.length); + + auto mag_msg = std::make_unique(); + + mag_msg->header.frame_id = this->frame_id_; + mag_msg->header.stamp = now; + + mag_msg->magnetic_field.x = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_magnetic_x.addr, + extern_control_table.imu_magnetic_x.length); + + mag_msg->magnetic_field.y = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_magnetic_y.addr, + extern_control_table.imu_magnetic_y.length); + + mag_msg->magnetic_field.z = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.imu_magnetic_z.addr, + extern_control_table.imu_magnetic_z.length); + + imu_pub_->publish(std::move(imu_msg)); + mag_pub_->publish(std::move(mag_msg)); +} diff --git a/turtlebot3_node/src/sensors/joint_state.cpp b/turtlebot3_node/src/sensors/joint_state.cpp new file mode 100644 index 00000000..e7673ef7 --- /dev/null +++ b/turtlebot3_node/src/sensors/joint_state.cpp @@ -0,0 +1,90 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include + +#include "turtlebot3_node/sensors/joint_state.hpp" + +using namespace robotis; +using namespace turtlebot3; + +sensors::JointState::JointState( + std::shared_ptr & nh, + const std::string & topic_name, + const std::string & frame_id) +: Sensors(nh, frame_id) +{ + pub_ = nh->create_publisher(topic_name, this->qos_); + + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create joint state publisher"); +} + +void sensors::JointState::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + auto msg = std::make_unique(); + + static std::array last_diff_position, last_position; + + std::array position = + {dxl_sdk_wrapper->get_data_from_device( + extern_control_table.present_position_left.addr, + extern_control_table.present_position_left.length), + dxl_sdk_wrapper->get_data_from_device( + extern_control_table.present_position_right.addr, + extern_control_table.present_position_right.length)}; + + std::array velocity = + {dxl_sdk_wrapper->get_data_from_device( + extern_control_table.present_velocity_left.addr, + extern_control_table.present_velocity_left.length), + dxl_sdk_wrapper->get_data_from_device( + extern_control_table.present_velocity_right.addr, + extern_control_table.present_velocity_right.length)}; + + // std::array current = + // {dxl_sdk_wrapper->get_data_from_device( + // extern_control_table.resent_current_left.addr, + // extern_control_table.resent_current_left.length), + // dxl_sdk_wrapper->get_data_from_device( + // extern_control_table.resent_current_right.addr, + // extern_control_table.resent_current_right.length)}; + + msg->header.frame_id = this->frame_id_; + msg->header.stamp = now; + + msg->name.push_back("wheel_left_joint"); + msg->name.push_back("wheel_right_joint"); + + msg->position.push_back(TICK_TO_RAD * last_diff_position[0]); + msg->position.push_back(TICK_TO_RAD * last_diff_position[1]); + + msg->velocity.push_back(RPM_TO_MS * velocity[0]); + msg->velocity.push_back(RPM_TO_MS * velocity[1]); + + // msg->effort.push_back(current[0]); + // msg->effort.push_back(current[1]); + + last_diff_position[0] += (position[0] - last_position[0]); + last_diff_position[1] += (position[1] - last_position[1]); + + last_position = position; + + pub_->publish(std::move(msg)); +} diff --git a/turtlebot3_node/src/sensors/sensor_state.cpp b/turtlebot3_node/src/sensors/sensor_state.cpp new file mode 100644 index 00000000..bffcda4c --- /dev/null +++ b/turtlebot3_node/src/sensors/sensor_state.cpp @@ -0,0 +1,157 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/sensors/sensor_state.hpp" + +using namespace robotis; +using namespace turtlebot3; + +sensors::SensorState::SensorState( + std::shared_ptr & nh, + const std::string & topic_name, + const bool & bumper_forward, + const bool & bumper_backward, + const bool & cliff, + const bool & sonar, + const bool & illumination) +: Sensors(nh), + bumper_forward_(bumper_forward), + bumper_backward_(bumper_backward), + cliff_(cliff), + sonar_(sonar), + illumination_(illumination) +{ + pub_ = nh->create_publisher(topic_name, this->qos_); + + RCLCPP_INFO(nh_->get_logger(), "Succeeded to create sensor state publisher"); +} + +void sensors::SensorState::publish( + const rclcpp::Time & now, + std::shared_ptr & dxl_sdk_wrapper) +{ + auto msg = std::make_unique(); + + msg->header.stamp = now; + + if (bumper_forward_ || bumper_backward_) + { + uint8_t bumper_push_state; + uint8_t bumper_forward_state; + uint8_t bumper_backward_state; + + bumper_forward_state = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.bumper_1.addr, + extern_control_table.bumper_1.length); + + bumper_backward_state = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.bumper_2.addr, + extern_control_table.bumper_2.length); + + bumper_push_state = bumper_forward_state<<0; + bumper_push_state |= bumper_backward_state<<1; + + msg->bumper = bumper_push_state; + } + else if (!bumper_forward_ && !bumper_backward_) + { + msg->bumper = 0; + } + + if (cliff_) + { + msg->cliff = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.ir.addr, + extern_control_table.ir.length); + } + else + { + msg->cliff = 0.0f; + } + + if (sonar_) + { + msg->sonar = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.sonar.addr, + extern_control_table.sonar.length); + } + else + { + msg->sonar = 0.0f; + } + + if (illumination_) + { + msg->illumination = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.illumination.addr, + extern_control_table.illumination.length); + } + else + { + msg->illumination = 0.0f; + } + + + if (sonar_) + { + msg->sonar = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.sonar.addr, + extern_control_table.sonar.length); + } + else + { + msg->sonar = 0.0f; + } + + { + uint8_t button_push_state; + uint8_t button_0_state; + uint8_t button_1_state; + + button_0_state = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.button_1.addr, + extern_control_table.button_1.length); + + button_1_state = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.button_2.addr, + extern_control_table.button_2.length); + + button_push_state = button_0_state<<0; + button_push_state |= button_1_state<<1; + + msg->button = button_push_state; + } + + msg->torque = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.motor_torque_enable.addr, + extern_control_table.motor_torque_enable.length); + + msg->left_encoder = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.present_position_left.addr, + extern_control_table.present_position_left.length); + + msg->right_encoder = dxl_sdk_wrapper->get_data_from_device( + extern_control_table.present_position_right.addr, + extern_control_table.present_position_right.length); + + msg->battery = 0.01f * dxl_sdk_wrapper->get_data_from_device( + extern_control_table.battery_voltage.addr, + extern_control_table.battery_voltage.length); + + pub_->publish(std::move(msg)); +} diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp new file mode 100644 index 00000000..f7148194 --- /dev/null +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -0,0 +1,354 @@ +/******************************************************************************* +* Copyright 2019 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. +*******************************************************************************/ + +/* Author: Darby Lim */ + +#include "turtlebot3_node/turtlebot3.hpp" + +using namespace robotis; +using namespace std::chrono_literals; +using namespace turtlebot3; + +TurtleBot3::TurtleBot3(const std::string & usb_port) +: Node("turtlebot3_node",rclcpp::NodeOptions().use_intra_process_comms(true)) +{ + RCLCPP_INFO(get_logger(), "Init TurtleBot3 Node Main"); + node_handle_ = std::shared_ptr<::rclcpp::Node>(this, [](::rclcpp::Node *) {}); + + init_dynamixel_sdk_wrapper(usb_port); + check_device_status(); + + add_motors(); + add_wheels(); + add_sensors(); + add_devices(); + + run(); +} + +TurtleBot3::Wheels* TurtleBot3::get_wheels() +{ + return &wheels_; +} + +TurtleBot3::Motors* TurtleBot3::get_motors() +{ + return &motors_; +} + +void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) +{ + DynamixelSDKWrapper::Device opencr = {usb_port, 200, 1000000, 2.0f}; + + this->declare_parameter("opencr.id"); + this->declare_parameter("opencr.baud_rate"); + this->declare_parameter("opencr.protocol_version"); + + this->get_parameter_or("opencr.id", opencr.id, 200); + this->get_parameter_or("opencr.baud_rate", opencr.baud_rate, 1000000); + this->get_parameter_or("opencr.protocol_version", opencr.protocol_version, 2.0f); + + RCLCPP_INFO(this->get_logger(), "Init DynamixelSDKWrapper"); + + dxl_sdk_wrapper_ = std::make_shared(opencr); + + dxl_sdk_wrapper_->init_read_memory( + extern_control_table.millis.addr, + (extern_control_table.profile_acceleration_right.addr - extern_control_table.millis.addr) + + extern_control_table.profile_acceleration_right.length + ); +} + +void TurtleBot3::check_device_status() +{ + if (dxl_sdk_wrapper_->is_connected_to_device()) + { + std::string sdk_msg; + uint8_t reset = 1; + + dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.imu_re_calibration.addr, + extern_control_table.imu_re_calibration.length, + &reset, + &sdk_msg); + + RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); + rclcpp::sleep_for(std::chrono::seconds(5)); + RCLCPP_INFO(this->get_logger(), "Calibration End"); + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); + rclcpp::shutdown(); + return; + } + + const int8_t NOT_CONNECTED_MOTOR = -1; + + int8_t device_status = dxl_sdk_wrapper_->get_data_from_device( + extern_control_table.device_status.addr, + extern_control_table.device_status.length); + + switch (device_status) + { + case NOT_CONNECTED_MOTOR: + RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); + break; + + default: + break; + } +} + +void TurtleBot3::add_motors() +{ + RCLCPP_INFO(this->get_logger(), "Add Motors"); + + this->declare_parameter("motors.profile_acceleration_constant"); + this->declare_parameter("motors.profile_acceleration"); + + this->get_parameter_or( + "motors.profile_acceleration_constant", + motors_.profile_acceleration_constant, + 214.577); + + this->get_parameter_or( + "motors.profile_acceleration", + motors_.profile_acceleration, + 0.0); +} + +void TurtleBot3::add_wheels() +{ + RCLCPP_INFO(this->get_logger(), "Add Wheels"); + + this->declare_parameter("wheels.separation"); + this->declare_parameter("wheels.radius"); + + this->get_parameter_or("wheels.separation", wheels_.separation, 0.160); + this->get_parameter_or("wheels.radius", wheels_.radius, 0.033); +} + +void TurtleBot3::add_sensors() +{ + RCLCPP_INFO(this->get_logger(), "Add Sensors"); + + 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"); + + bool is_connected_bumper_1 = this->get_parameter("sensors.bumper_1").as_bool(); + bool is_connected_bumper_2 = this->get_parameter("sensors.bumper_2").as_bool(); + + bool is_connected_illumination = this->get_parameter("sensors.illumination").as_bool(); + + bool is_connected_ir = this->get_parameter("sensors.ir").as_bool(); + + bool is_connected_sonar = this->get_parameter("sensors.sonar").as_bool(); + + sensors_.push_back(new sensors::BatteryState( + node_handle_, + "battery_state")); + + sensors_.push_back(new sensors::Imu( + node_handle_, + "imu", + "magnetic_field", + "imu_link")); + + sensors_.push_back(new sensors::SensorState( + node_handle_, + "sensor_state", + is_connected_bumper_1, + is_connected_bumper_2, + is_connected_illumination, + is_connected_ir, + is_connected_sonar)); + + sensors_.push_back(new sensors::JointState(node_handle_, "joint_states", "base_link")); +} + +void TurtleBot3::add_devices() +{ + RCLCPP_INFO(this->get_logger(), "Add Devices"); + devices_["motor_power"] = + new devices::MotorPower(node_handle_, dxl_sdk_wrapper_, "motor_power"); + devices_["reset"] = + new devices::Reset(node_handle_, dxl_sdk_wrapper_, "reset"); + devices_["sound"] = + new devices::Sound(node_handle_, dxl_sdk_wrapper_, "sound"); +} + +void TurtleBot3::run() +{ + RCLCPP_INFO(this->get_logger(), "Run!"); + + publish_timer(std::chrono::milliseconds(50)); + heartbeat_timer(std::chrono::milliseconds(100)); + + parameter_event_callback(); + cmd_vel_callback(); +} + +void TurtleBot3::publish_timer(const std::chrono::milliseconds timeout) +{ + publish_timer_ = this->create_wall_timer( + timeout, + [this]() -> void + { + rclcpp::Time now = this->now(); + + dxl_sdk_wrapper_->read_data_set(); + + for (const auto &sensor:sensors_) + { + sensor->publish(now, dxl_sdk_wrapper_); + } + } + ); +} + +void TurtleBot3::heartbeat_timer(const std::chrono::milliseconds timeout) +{ + heartbeat_timer_ = this->create_wall_timer( + timeout, + [this]() -> void + { + static uint8_t count = 0; + std::string msg; + + dxl_sdk_wrapper_->set_data_to_device( + extern_control_table.heartbeat.addr, + extern_control_table.heartbeat.length, + &count, + &msg); + + RCLCPP_DEBUG(this->get_logger(), "hearbeat count : %d, msg : %s", count, msg.c_str()); + + count++; + } + ); +} + +void TurtleBot3::parameter_event_callback() +{ + priv_parameters_client_ = std::make_shared(this); + while (!priv_parameters_client_->wait_for_service(std::chrono::seconds(1))) + { + if (!rclcpp::ok()) + { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting."); + return; + } + + RCLCPP_WARN(this->get_logger(), "service not available, waiting again..."); + } + + auto param_event_callback = + [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void + { + for (const auto & changed_parameter : event->changed_parameters) + { + RCLCPP_DEBUG( + this->get_logger(), + "changed parameter name : %s", + changed_parameter.name.c_str()); + + if (changed_parameter.name == "motors.profile_acceleration") + { + std::string sdk_msg; + + motors_.profile_acceleration = + rclcpp::Parameter::from_parameter_msg(changed_parameter).as_double(); + + motors_.profile_acceleration = + motors_.profile_acceleration / motors_.profile_acceleration_constant; + + union Data + { + int32_t dword[2]; + uint8_t byte[4*2]; + } data; + + data.dword[0] = static_cast(motors_.profile_acceleration); + data.dword[1] = static_cast(motors_.profile_acceleration); + + uint16_t start_addr = extern_control_table.profile_acceleration_left.addr; + uint16_t addr_length = + (extern_control_table.profile_acceleration_right.addr - + extern_control_table.profile_acceleration_left.addr) + + extern_control_table.profile_acceleration_right.length; + + uint8_t * p_data = &data.byte[0]; + + dxl_sdk_wrapper_->set_data_to_device(start_addr, addr_length, p_data, &sdk_msg); + + RCLCPP_INFO( + this->get_logger(), + "changed parameter value : %f [rev/min2] sdk_msg : %s", + motors_.profile_acceleration, + sdk_msg.c_str()); + } + } + }; + + parameter_event_sub_ = priv_parameters_client_->on_parameter_event(param_event_callback); +} + +void TurtleBot3::cmd_vel_callback() +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)); + cmd_vel_sub_ = this->create_subscription( + "cmd_vel", + qos, + [this](const geometry_msgs::msg::Twist::SharedPtr msg) -> void + { + std::string sdk_msg; + + union Data + { + int32_t dword[6]; + uint8_t byte[4*6]; + } data; + + data.dword[0] = static_cast(msg->linear.x * 100); + data.dword[1] = 0; + data.dword[2] = 0; + data.dword[3] = 0; + data.dword[4] = 0; + data.dword[5] = static_cast(msg->angular.z * 100); + + uint16_t start_addr = extern_control_table.cmd_velocity_linear_x.addr; + uint16_t addr_length = + (extern_control_table.cmd_velocity_angular_z.addr - + extern_control_table.cmd_velocity_linear_x.addr) + + extern_control_table.cmd_velocity_angular_z.length; + + uint8_t * p_data = &data.byte[0]; + + dxl_sdk_wrapper_->set_data_to_device(start_addr, addr_length, p_data, &sdk_msg); + + RCLCPP_DEBUG( + this->get_logger(), + "lin_vel: %f ang_vel: %f msg : %s", msg->linear.x, msg->angular.z, sdk_msg.c_str()); + } + ); +} diff --git a/turtlebot3_sbc_settings/crystal/tb3_sbc_settings.tar.bz2 b/turtlebot3_sbc_settings/crystal/tb3_sbc_settings.tar.bz2 deleted file mode 100644 index f870ff9f..00000000 Binary files a/turtlebot3_sbc_settings/crystal/tb3_sbc_settings.tar.bz2 and /dev/null differ diff --git a/turtlebot3_sbc_settings/dashing/tb3_sbc_settings.tar.bz2 b/turtlebot3_sbc_settings/dashing/tb3_sbc_settings.tar.bz2 deleted file mode 100644 index 9ed332d7..00000000 Binary files a/turtlebot3_sbc_settings/dashing/tb3_sbc_settings.tar.bz2 and /dev/null differ diff --git a/turtlebot3_sbc_settings/latest/tb3_sbc_settings.tar.bz2 b/turtlebot3_sbc_settings/latest/tb3_sbc_settings.tar.bz2 deleted file mode 100644 index 9ed332d7..00000000 Binary files a/turtlebot3_sbc_settings/latest/tb3_sbc_settings.tar.bz2 and /dev/null differ diff --git a/turtlebot3_teleop/CHANGELOG.rst b/turtlebot3_teleop/CHANGELOG.rst new file mode 100644 index 00000000..941310e6 --- /dev/null +++ b/turtlebot3_teleop/CHANGELOG.rst @@ -0,0 +1,60 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package turtlebot3_teleop +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2019-08-20) +------------------ +* Supported ROS 2 Dashing Diademata +* Updated the CHANGELOG and version to release binary packages +* Contributors: Darby Lim, Pyo + +1.2.2 (2019-08-20) +------------------ +* none + +1.2.1 (2019-08-20) +------------------ +* none + +1.2.0 (2019-01-22) +------------------ +* added windows port `#358 `_ +* Contributors: Sean Yen, Taehun Lim + +1.1.0 (2018-07-23) +------------------ +* none + +1.0.0 (2018-05-29) +------------------ +* added constrain to limit velocity +* modified initial value, profile function, limit velocity msg +* modified global names `#211 `_ from FurqanHabibi/fix_global_topic_name +* Contributors: Darby Lim, Muhammad Furqan Habibi, Pyo + +0.2.1 (2018-03-14) +------------------ +* none + +0.2.0 (2018-03-12) +------------------ +* refactoring for release +* Contributors: Pyo + +0.1.6 (2017-08-14) +------------------ +* none + +0.1.5 (2017-05-25) +------------------ +* none + +0.1.4 (2017-05-23) +------------------ +* modified launch file name +* added teleop package +* Contributors: Darby Lim + +0.1.3 (2017-04-24) +------------------ +* none diff --git a/turtlebot3_teleop/package.xml b/turtlebot3_teleop/package.xml index c3ff7314..5bc6d21d 100644 --- a/turtlebot3_teleop/package.xml +++ b/turtlebot3_teleop/package.xml @@ -1,21 +1,23 @@ - + + turtlebot3_teleop - 0.0.1 + 2.0.0 - Provides teleoperation using keyboard for TurtleBot3. + Teleoperation node using keyboard for TurtleBot3. - Apache 2.0 Darby Lim - Pyo + Pyo Pyo - http://wiki.ros.org/turtlebot3_description + Apache 2.0 + http://wiki.ros.org/turtlebot3_teleop http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues - rclpy - rclpy - geometry_msgs + geometry_msgs + launch + launch_ros + rclpy ament_python diff --git a/turtlebot3_teleop/setup.py b/turtlebot3_teleop/setup.py index fe581a40..73eb8ec7 100644 --- a/turtlebot3_teleop/setup.py +++ b/turtlebot3_teleop/setup.py @@ -8,11 +8,10 @@ setup( name=package_name, - version='0.0.1', + version='2.0.0', packages=find_packages(exclude=[]), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ], install_requires=[ @@ -31,7 +30,7 @@ 'Topic :: Software Development', ], description=( - 'Provides teleoperation using keyboard for TurtleBot3.' + 'Teleoperation node using keyboard for TurtleBot3.' ), license='Apache License, Version 2.0', tests_require=['pytest'], diff --git a/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py b/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py index 9ee10a3f..aa833a50 100755 --- a/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py +++ b/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py @@ -26,13 +26,16 @@ # POSSIBILITY OF SUCH DAMAGE. # /* Author: Darby Lim */ -import os, sys + +import os import select +import sys import termios import tty -import rclpy from geometry_msgs.msg import Twist +import rclpy +from rclpy.qos import QoSProfile BURGER_MAX_LIN_VEL = 0.22 BURGER_MAX_ANG_VEL = 2.84 @@ -65,7 +68,8 @@ Communications Failed """ -def getKey(settings): + +def get_key(settings): tty.setraw(sys.stdin.fileno()) rlist, _, _ = select.select([sys.stdin], [], [], 0.1) if rlist: @@ -76,10 +80,14 @@ def getKey(settings): termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key -def vels(target_linear_vel, target_angular_vel): - return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel) -def makeSimpleProfile(output, input, slop): +def print_vels(target_linear_velocity, target_angular_velocity): + print('currently:\tlinear velocity {0}\t angular velocity {1} '.format( + target_linear_velocity, + target_angular_velocity)) + + +def make_simple_profile(output, input, slop): if input > output: output = min( input, output + slop ) elif input < output: @@ -89,73 +97,76 @@ def makeSimpleProfile(output, input, slop): return output -def constrain(input, low, high): - if input < low: - input = low - elif input > high: - input = high + +def constrain(input_vel, low_bound, high_bound): + if input_vel < low_bound: + input_vel = low_bound + elif input_vel > high_bound: + input_vel = high_bound else: - input = input + input_vel = input_vel - return input + return input_vel -def checkLinearLimitVelocity(vel): + +def check_linear_limit_velocity(velocity): if TURTLEBOT3_MODEL == 'burger': - vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL) + return constrain(velocity, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL) else: - vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL) - - return vel + return constrain(velocity, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL) -def checkAngularLimitVelocity(vel): +def check_angular_limit_velocity(velocity): if TURTLEBOT3_MODEL == 'burger': - vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL) + return constrain(velocity, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL) else: - vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL) - + return constrain(velocity, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL) - return vel def main(): settings = termios.tcgetattr(sys.stdin) rclpy.init() + qos = QoSProfile(depth=10) node = rclpy.create_node('teleop_keyboard') - pub = node.create_publisher(Twist, 'cmd_vel', 10) + pub = node.create_publisher(Twist, 'cmd_vel', qos) status = 0 - target_linear_vel = 0.0 - target_angular_vel = 0.0 - control_linear_vel = 0.0 - control_angular_vel = 0.0 + target_linear_velocity = 0.0 + target_angular_velocity = 0.0 + control_linear_velocity = 0.0 + control_angular_velocity = 0.0 try: print(msg) while(1): - key = getKey(settings) + key = get_key(settings) if key == 'w' : - target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE) + target_linear_velocity =\ + check_linear_limit_velocity(target_linear_velocity + LIN_VEL_STEP_SIZE) status = status + 1 - print(vels(target_linear_vel,target_angular_vel)) + print_vels(target_linear_velocity, target_angular_velocity) elif key == 'x' : - target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE) + target_linear_velocity =\ + check_linear_limit_velocity(target_linear_velocity - LIN_VEL_STEP_SIZE) status = status + 1 - print(vels(target_linear_vel,target_angular_vel)) + print_vels(target_linear_velocity, target_angular_velocity) elif key == 'a' : - target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE) + target_angular_velocity =\ + check_angular_limit_velocity(target_angular_velocity + ANG_VEL_STEP_SIZE) status = status + 1 - print(vels(target_linear_vel,target_angular_vel)) + print_vels(target_linear_velocity, target_angular_velocity) elif key == 'd' : - target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE) + target_angular_velocity =\ + check_angular_limit_velocity(target_angular_velocity - ANG_VEL_STEP_SIZE) status = status + 1 - print(vels(target_linear_vel,target_angular_vel)) + print_vels(target_linear_velocity, target_angular_velocity) elif key == ' ' or key == 's' : - target_linear_vel = 0.0 - control_linear_vel = 0.0 - target_angular_vel = 0.0 - control_angular_vel = 0.0 - print(vels(target_linear_vel, target_angular_vel)) + target_linear_velocity = 0.0 + control_linear_velocity = 0.0 + target_angular_velocity = 0.0 + control_angular_velocity = 0.0 + print_vels(target_linear_velocity, target_angular_velocity) else: if (key == '\x03'): break @@ -166,11 +177,23 @@ def main(): twist = Twist() - control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0)) - twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0 + control_linear_velocity = make_simple_profile( + control_linear_velocity, + target_linear_velocity, + (LIN_VEL_STEP_SIZE/2.0)) + + twist.linear.x = control_linear_velocity + twist.linear.y = 0.0 + twist.linear.z = 0.0 - control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0)) - twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel + control_angular_velocity = make_simple_profile( + control_angular_velocity, + target_angular_velocity, + (ANG_VEL_STEP_SIZE/2.0)) + + twist.angular.x = 0.0 + twist.angular.y = 0.0 + twist.angular.z = control_angular_velocity pub.publish(twist) @@ -179,8 +202,14 @@ def main(): finally: twist = Twist() - twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0 - twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0 + twist.linear.x = 0.0 + twist.linear.y = 0.0 + twist.linear.z = 0.0 + + twist.angular.x = 0.0 + twist.angular.y = 0.0 + twist.angular.z = 0.0 + pub.publish(twist) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)