diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml new file mode 100644 index 00000000..f66e631e --- /dev/null +++ b/.github/workflows/lint.yml @@ -0,0 +1,57 @@ +name: Lint + +# Controls when the action will run. Triggers the workflow on push or pull request +on: + push: + branches: [ ros2, ros2-devel, dashing-devel, eloquent-devel, foxy-devel ] + pull_request: + branches: [ ros2, ros2-devel ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + ament_lint_bionic: + runs-on: ubuntu-18.04 + strategy: + fail-fast: false + matrix: + linter: [copyright, cppcheck, cpplint, uncrustify, pep8, pep257, xmllint] + distribution: [dashing, eloquent] + steps: + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master + with: + distribution: ${{ matrix.distribution }} + linter: ${{ matrix.linter }} + package-name: | + turtlebot3 + turtlebot3_bringup + turtlebot3_cartographer + turtlebot3_description + turtlebot3_example + turtlebot3_navigation2 + turtlebot3_node + turtlebot3_teleop + ament_lint_focal: + runs-on: ubuntu-20.04 + strategy: + fail-fast: false + matrix: + linter: [copyright, cppcheck, cpplint, uncrustify, pep257, xmllint] #todo : flake8 + distribution: [foxy] + steps: + - uses: actions/checkout@v2 + - uses: ros-tooling/setup-ros@master + - uses: ros-tooling/action-ros-lint@master + with: + distribution: ${{ matrix.distribution }} + linter: ${{ matrix.linter }} + package-name: | + turtlebot3 + turtlebot3_bringup + turtlebot3_cartographer + turtlebot3_description + turtlebot3_example + turtlebot3_navigation2 + turtlebot3_node + turtlebot3_teleop diff --git a/.travis.yml b/.travis.yml index 1e5a3872..f72e301f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -26,4 +26,4 @@ matrix: include: - script: .ros2ci/travis.bash dashing - script: .ros2ci/travis.bash eloquent - - script: .ros2ci/travis.bash nightly + - script: .ros2ci/travis.bash foxy diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..cfba094d --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/LICENSE b/LICENSE index 8dada3ed..f4f87bd4 100644 --- a/LICENSE +++ b/LICENSE @@ -1,3 +1,4 @@ + Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ @@ -178,7 +179,7 @@ APPENDIX: How to apply the Apache License to your work. To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "{}" + boilerplate notice, with the fields enclosed by brackets "[]" replaced with your own identifying information. (Don't include the brackets!) The text should be enclosed in the appropriate comment syntax for the file format. We also recommend that a @@ -186,7 +187,7 @@ same "printed page" as the copyright notice for easier identification within third-party archives. - Copyright {yyyy} {name of copyright owner} + Copyright [yyyy] [name of copyright owner] Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. @@ -199,3 +200,4 @@ 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. + \ No newline at end of file diff --git a/turtlebot3/package.xml b/turtlebot3/package.xml index 641726de..f6710115 100644 --- a/turtlebot3/package.xml +++ b/turtlebot3/package.xml @@ -6,15 +6,14 @@ ROS 2 packages for TurtleBot3 + Will Son Apache 2.0 + http://turtlebot3.robotis.com + https://github.com/ROBOTIS-GIT/turtlebot3 + https://github.com/ROBOTIS-GIT/turtlebot3/issues Darby Lim Ryan Shim Pyo - Will Son - http://wiki.ros.org/turtlebot3_description - http://turtlebot3.robotis.com - https://github.com/ROBOTIS-GIT/turtlebot3 - https://github.com/ROBOTIS-GIT/turtlebot3/issues ament_cmake turtlebot3_bringup turtlebot3_cartographer diff --git a/turtlebot3_bringup/launch/robot.launch.py b/turtlebot3_bringup/launch/robot.launch.py index 2c496bb6..863b92f6 100644 --- a/turtlebot3_bringup/launch/robot.launch.py +++ b/turtlebot3_bringup/launch/robot.launch.py @@ -48,9 +48,9 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument( - 'use_sim_time', - default_value=use_sim_time, - description='Use simulation (Gazebo) clock if true'), + 'use_sim_time', + default_value=use_sim_time, + description='Use simulation (Gazebo) clock if true'), DeclareLaunchArgument( 'usb_port', diff --git a/turtlebot3_bringup/launch/rviz2.launch.py b/turtlebot3_bringup/launch/rviz2.launch.py index 75afe6b4..810c5b26 100644 --- a/turtlebot3_bringup/launch/rviz2.launch.py +++ b/turtlebot3_bringup/launch/rviz2.launch.py @@ -25,9 +25,9 @@ def generate_launch_description(): rviz_config_dir = os.path.join( - get_package_share_directory('turtlebot3_description'), - 'rviz', - 'model.rviz') + get_package_share_directory('turtlebot3_description'), + 'rviz', + 'model.rviz') return LaunchDescription([ Node( diff --git a/turtlebot3_bringup/package.xml b/turtlebot3_bringup/package.xml index 457a2c78..da6b4b15 100644 --- a/turtlebot3_bringup/package.xml +++ b/turtlebot3_bringup/package.xml @@ -6,14 +6,13 @@ ROS 2 launch scripts for starting the TurtleBot3 - Apache 2.0 - Darby Lim - Pyo Will Son - http://wiki.ros.org/turtlebot3_bringup - http://turtlebot3.robotis.com + Apache 2.0 + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Darby Lim + Pyo ament_cmake hls_lfcd_lds_driver robot_state_publisher diff --git a/turtlebot3_cartographer/launch/cartographer.launch.py b/turtlebot3_cartographer/launch/cartographer.launch.py index 16f6542c..b6de9558 100644 --- a/turtlebot3_cartographer/launch/cartographer.launch.py +++ b/turtlebot3_cartographer/launch/cartographer.launch.py @@ -11,7 +11,7 @@ # 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 import os @@ -24,17 +24,20 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ThisLaunchFileDir + def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') turtlebot3_cartographer_prefix = get_package_share_directory('turtlebot3_cartographer') - cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', - default=os.path.join(turtlebot3_cartographer_prefix, 'config')) - configuration_basename = LaunchConfiguration('configuration_basename', default='turtlebot3_lds_2d.lua') + cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join( + turtlebot3_cartographer_prefix, 'config')) + configuration_basename = LaunchConfiguration('configuration_basename', + default='turtlebot3_lds_2d.lua') resolution = LaunchConfiguration('resolution', default='0.05') publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0') - rviz_config_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'), 'rviz', 'tb3_cartographer.rviz') + rviz_config_dir = os.path.join(get_package_share_directory('turtlebot3_cartographer'), + 'rviz', 'tb3_cartographer.rviz') return LaunchDescription([ DeclareLaunchArgument( @@ -56,7 +59,8 @@ def generate_launch_description(): node_name='cartographer_node', output='screen', parameters=[{'use_sim_time': use_sim_time}], - arguments=['-configuration_directory', cartographer_config_dir, '-configuration_basename', configuration_basename]), + arguments=['-configuration_directory', cartographer_config_dir, + '-configuration_basename', configuration_basename]), DeclareLaunchArgument( 'resolution', @@ -70,7 +74,8 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/occupancy_grid.launch.py']), - launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution, 'publish_period_sec': publish_period_sec}.items(), + launch_arguments={'use_sim_time': use_sim_time, 'resolution': resolution, + 'publish_period_sec': publish_period_sec}.items(), ), Node( diff --git a/turtlebot3_cartographer/launch/occupancy_grid.launch.py b/turtlebot3_cartographer/launch/occupancy_grid.launch.py index 175bb116..9a8cc400 100644 --- a/turtlebot3_cartographer/launch/occupancy_grid.launch.py +++ b/turtlebot3_cartographer/launch/occupancy_grid.launch.py @@ -11,7 +11,7 @@ # 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 from launch import LaunchDescription @@ -19,6 +19,7 @@ from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration + def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') resolution = LaunchConfiguration('resolution', default='0.05') diff --git a/turtlebot3_cartographer/package.xml b/turtlebot3_cartographer/package.xml index 8a9837cd..7c8eee83 100644 --- a/turtlebot3_cartographer/package.xml +++ b/turtlebot3_cartographer/package.xml @@ -6,14 +6,13 @@ ROS 2 launch scripts for cartographer - Apache 2.0 - Darby Lim - Pyo Will Son - http://wiki.ros.org/turtlebot3_bringup - http://turtlebot3.robotis.com + Apache 2.0 + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Darby Lim + Pyo ament_cmake cartographer_ros diff --git a/turtlebot3_description/package.xml b/turtlebot3_description/package.xml index 4092a7b6..39893c54 100644 --- a/turtlebot3_description/package.xml +++ b/turtlebot3_description/package.xml @@ -6,14 +6,13 @@ 3D models of the TurtleBot3 for simulation and visualization - Apache 2.0 - Darby Lim - Pyo Will Son - http://wiki.ros.org/turtlebot3_description - http://turtlebot3.robotis.com + Apache 2.0 + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Darby Lim + Pyo ament_cmake urdf diff --git a/turtlebot3_example/package.xml b/turtlebot3_example/package.xml index 002aa82e..463fc19e 100644 --- a/turtlebot3_example/package.xml +++ b/turtlebot3_example/package.xml @@ -6,14 +6,13 @@ This package provides four basic examples for TurtleBot3 (i.e., interactive marker, object detection, patrol and position control). - Ryan Shim - Gilbert Will Son Apache 2.0 - http://wiki.ros.org/turtlebot3_example - http://turtlebot3.robotis.com + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Ryan Shim + Gilbert ament_cmake geometry_msgs nav_msgs diff --git a/turtlebot3_example/setup.py b/turtlebot3_example/setup.py index 2cea807b..bfc5be20 100644 --- a/turtlebot3_example/setup.py +++ b/turtlebot3_example/setup.py @@ -1,6 +1,3 @@ -import glob -import os - from setuptools import find_packages from setuptools import setup @@ -14,11 +11,14 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), # To be added - # ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', 'turtlebot3_interactive_marker.launch.py'))), - # ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', 'turtlebot3_obstacle_detection.launch.py'))), - # ('share/' + package_name + '/rviz', glob.glob(os.path.join('rviz', 'turtlebot3_interactive_marker.rviz'))), + # ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', + # 'turtlebot3_interactive_marker.launch.py'))), + # ('share/' + package_name + '/launch', glob.glob(os.path.join('launch', + # 'turtlebot3_obstacle_detection.launch.py'))), + # ('share/' + package_name + '/rviz', glob.glob(os.path.join('rviz', + # 'turtlebot3_interactive_marker.rviz'))), ], - install_requires=['setuptools','launch'], + install_requires=['setuptools', 'launch'], zip_safe=True, author=['Ryan Shim', 'Gilbert'], author_email=['jhshim@robotis.com', 'kkjong@robotis.com'], @@ -38,11 +38,16 @@ entry_points={ 'console_scripts': [ # To be added - # 'turtlebot3_interactive_marker = turtlebot3_example.turtlebot3_interactive_marker.main:main', - 'turtlebot3_obstacle_detection = turtlebot3_example.turtlebot3_obstacle_detection.main:main', - 'turtlebot3_patrol_client = turtlebot3_example.turtlebot3_patrol_client.main:main', - 'turtlebot3_patrol_server = turtlebot3_example.turtlebot3_patrol_server.main:main', - 'turtlebot3_position_control = turtlebot3_example.turtlebot3_position_control.main:main', + # 'turtlebot3_interactive_marker = \ + # turtlebot3_example.turtlebot3_interactive_marker.main:main', + 'turtlebot3_obstacle_detection = \ + turtlebot3_example.turtlebot3_obstacle_detection.main:main', + 'turtlebot3_patrol_client = \ + turtlebot3_example.turtlebot3_patrol_client.main:main', + 'turtlebot3_patrol_server = \ + turtlebot3_example.turtlebot3_patrol_server.main:main', + 'turtlebot3_position_control = \ + turtlebot3_example.turtlebot3_position_control.main:main', ], }, ) diff --git a/turtlebot3_example/turtlebot3_example/turtlebot3_obstacle_detection/turtlebot3_obstacle_detection.py b/turtlebot3_example/turtlebot3_example/turtlebot3_obstacle_detection/turtlebot3_obstacle_detection.py index bc556ab2..f1dda6f3 100755 --- a/turtlebot3_example/turtlebot3_example/turtlebot3_obstacle_detection/turtlebot3_obstacle_detection.py +++ b/turtlebot3_example/turtlebot3_example/turtlebot3_obstacle_detection/turtlebot3_obstacle_detection.py @@ -16,8 +16,6 @@ # # Authors: Ryan Shim, Gilbert -import numpy - from geometry_msgs.msg import Twist from rclpy.node import Node from rclpy.qos import QoSProfile diff --git a/turtlebot3_example/turtlebot3_example/turtlebot3_position_control/turtlebot3_position_control.py b/turtlebot3_example/turtlebot3_example/turtlebot3_position_control/turtlebot3_position_control.py index 7850a916..27cb72f6 100755 --- a/turtlebot3_example/turtlebot3_example/turtlebot3_position_control/turtlebot3_position_control.py +++ b/turtlebot3_example/turtlebot3_example/turtlebot3_position_control/turtlebot3_position_control.py @@ -108,8 +108,8 @@ def generate_path(self): # Step 1: Turn if self.step == 1: path_theta = math.atan2( - self.goal_pose_y-self.last_pose_y, - self.goal_pose_x-self.last_pose_x) + self.goal_pose_y - self.last_pose_y, + self.goal_pose_x - self.last_pose_x) angle = path_theta - self.last_pose_theta angular_velocity = 0.1 # unit: rad/s @@ -118,8 +118,8 @@ def generate_path(self): # Step 2: Go Straight elif self.step == 2: distance = math.sqrt( - (self.goal_pose_x-self.last_pose_x)**2 - + (self.goal_pose_y-self.last_pose_y)**2) + (self.goal_pose_x - self.last_pose_x)**2 + + (self.goal_pose_y - self.last_pose_y)**2) linear_velocity = 0.1 # unit: m/s twist, self.step = Turtlebot3Path.go_straight(distance, linear_velocity, self.step) @@ -159,7 +159,8 @@ def get_key(self): *******************************************************************************""" def euler_from_quaternion(self, quat): """ - Converts quaternion (w in last place) to euler roll, pitch, yaw + Convert quaternion (w in last place) to euler roll, pitch, yaw. + quat = [x, y, z, w] """ x = quat.x @@ -167,15 +168,15 @@ def euler_from_quaternion(self, quat): z = quat.z w = quat.w - sinr_cosp = 2 * (w*x + y*z) - cosr_cosp = 1 - 2*(x*x + y*y) + sinr_cosp = 2 * (w * x + y * z) + cosr_cosp = 1 - 2 * (x * x + y * y) roll = numpy.arctan2(sinr_cosp, cosr_cosp) - sinp = 2 * (w*y - z*x) + sinp = 2 * (w * y - z * x) pitch = numpy.arcsin(sinp) - siny_cosp = 2 * (w*z + x*y) - cosy_cosp = 1 - 2 * (y*y + z*z) + siny_cosp = 2 * (w * z + x * y) + cosy_cosp = 1 - 2 * (y * y + z * z) yaw = numpy.arctan2(siny_cosp, cosy_cosp) return roll, pitch, yaw diff --git a/turtlebot3_navigation2/launch/navigation2.launch.py b/turtlebot3_navigation2/launch/navigation2.launch.py index 027a60b7..2e145568 100644 --- a/turtlebot3_navigation2/launch/navigation2.launch.py +++ b/turtlebot3_navigation2/launch/navigation2.launch.py @@ -11,7 +11,7 @@ # 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 import os @@ -26,6 +26,7 @@ TURTLEBOT3_MODEL = os.environ['TURTLEBOT3_MODEL'] + def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time', default='false') map_dir = LaunchConfiguration( diff --git a/turtlebot3_navigation2/package.xml b/turtlebot3_navigation2/package.xml index 5764cf77..342fb612 100644 --- a/turtlebot3_navigation2/package.xml +++ b/turtlebot3_navigation2/package.xml @@ -5,15 +5,14 @@ 2.1.0 ROS 2 launch scripts for navigation2 - - Apache 2.0 - Darby Lim - Pyo + Will Son - http://wiki.ros.org/turtlebot3_bringup - http://turtlebot3.robotis.com + Apache 2.0 + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Darby Lim + Pyo ament_cmake nav2_bringup diff --git a/turtlebot3_node/include/turtlebot3_node/control_table.hpp b/turtlebot3_node/include/turtlebot3_node/control_table.hpp index 55130b83..35e7e42c 100644 --- a/turtlebot3_node/include/turtlebot3_node/control_table.hpp +++ b/turtlebot3_node/include/turtlebot3_node/control_table.hpp @@ -1,23 +1,21 @@ -/******************************************************************************* -* 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_ +// 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 @@ -110,7 +108,7 @@ typedef struct } ControlTable; const ControlTable extern_control_table; -} // turtlebot3 -} // robotis +} // namespace turtlebot3 +} // namespace robotis -#endif // TURTLEBOT3_NODE_CONTROL_TABLE_HPP_ +#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 index 6161f9f5..3a675665 100644 --- a/turtlebot3_node/include/turtlebot3_node/devices/devices.hpp +++ b/turtlebot3_node/include/turtlebot3_node/devices/devices.hpp @@ -1,30 +1,28 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ +// 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_ -/* Author: Darby Lim */ - -#ifndef TURTLEBOT3_NODE_DEVICES_DEVICES_HPP_ -#define TURTLEBOT3_NODE_DEVICES_DEVICES_HPP_ +#include #include #include #include -#include - #include "turtlebot3_node/control_table.hpp" #include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" @@ -35,8 +33,9 @@ namespace turtlebot3 extern const ControlTable extern_control_table; namespace devices { -class Devices{ - public: +class Devices +{ +public: explicit Devices( std::shared_ptr & nh, std::shared_ptr & dxl_sdk_wrapper) @@ -47,12 +46,12 @@ class Devices{ virtual void command(const void * request, void * response) = 0; - protected: +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_ +} // namespace devices +} // namespace turtlebot3 +} // namespace 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 index ab0b9af5..9c8159e9 100644 --- a/turtlebot3_node/include/turtlebot3_node/devices/motor_power.hpp +++ b/turtlebot3_node/include/turtlebot3_node/devices/motor_power.hpp @@ -1,26 +1,27 @@ -/******************************************************************************* -* 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_ +// 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 +#include + #include "turtlebot3_node/devices/devices.hpp" namespace robotis @@ -31,7 +32,7 @@ namespace devices { class MotorPower : public Devices { - public: +public: static void request( rclcpp::Client::SharedPtr client, std_srvs::srv::SetBool::Request req); @@ -43,10 +44,10 @@ class MotorPower : public Devices void command(const void * request, void * response) override; - private: +private: rclcpp::Service::SharedPtr srv_; }; -} // devices -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_DEVICES_MOTOR_POWER_HPP_ +} // namespace devices +} // namespace turtlebot3 +} // namespace 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 index f83dc3a3..da6da70b 100644 --- a/turtlebot3_node/include/turtlebot3_node/devices/reset.hpp +++ b/turtlebot3_node/include/turtlebot3_node/devices/reset.hpp @@ -1,26 +1,27 @@ -/******************************************************************************* -* 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_ +// 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 +#include + #include "turtlebot3_node/devices/devices.hpp" namespace robotis @@ -31,7 +32,7 @@ namespace devices { class Reset : public Devices { - public: +public: static void request( rclcpp::Client::SharedPtr client, std_srvs::srv::Trigger::Request req); @@ -43,10 +44,10 @@ class Reset : public Devices void command(const void * request, void * response) override; - private: +private: rclcpp::Service::SharedPtr srv_; }; -} // devices -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_DEVICES_SOUND_HPP_ +} // namespace devices +} // namespace turtlebot3 +} // namespace robotis +#endif // TURTLEBOT3_NODE__DEVICES__RESET_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp b/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp index 6fc8a73d..99dcdd2b 100644 --- a/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp +++ b/turtlebot3_node/include/turtlebot3_node/devices/sound.hpp @@ -1,26 +1,27 @@ -/******************************************************************************* -* 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_ +// 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 +#include + #include "turtlebot3_node/devices/devices.hpp" namespace robotis @@ -31,7 +32,7 @@ namespace devices { class Sound : public Devices { - public: +public: static void request( rclcpp::Client::SharedPtr client, turtlebot3_msgs::srv::Sound::Request req); @@ -43,10 +44,10 @@ class Sound : public Devices void command(const void * request, void * response) override; - private: +private: rclcpp::Service::SharedPtr srv_; }; -} // devices -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_DEVICES_SOUND_HPP_ +} // namespace devices +} // namespace turtlebot3 +} // namespace robotis +#endif // TURTLEBOT3_NODE__DEVICES__SOUND_HPP_ diff --git a/turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp b/turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp index 0a7e89d4..9fe23bb1 100644 --- a/turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp +++ b/turtlebot3_node/include/turtlebot3_node/diff_drive_controller.hpp @@ -1,28 +1,26 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ +// 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 -/* Author: Darby Lim */ +#ifndef TURTLEBOT3_NODE__DIFF_DRIVE_CONTROLLER_HPP_ +#define TURTLEBOT3_NODE__DIFF_DRIVE_CONTROLLER_HPP_ -#ifndef TURTLEBOT3_NODE_DIFF_DRIVE_CONTROLLER_HPP_ -#define TURTLEBOT3_NODE_DIFF_DRIVE_CONTROLLER_HPP_ +#include #include -#include - #include "turtlebot3_node/odometry.hpp" namespace robotis @@ -31,14 +29,14 @@ namespace turtlebot3 { class DiffDriveController : public rclcpp::Node { - public: +public: explicit DiffDriveController(const float wheel_seperation, const float wheel_radius); - virtual ~DiffDriveController(){}; + virtual ~DiffDriveController() {} - private: +private: std::shared_ptr nh_; std::unique_ptr odometry_; }; -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_DIFF_DRIVE_CONTROLLER_HPP_ +} // namespace turtlebot3 +} // namespace 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 index 414705af..87e2bc6f 100644 --- a/turtlebot3_node/include/turtlebot3_node/dynamixel_sdk_wrapper.hpp +++ b/turtlebot3_node/include/turtlebot3_node/dynamixel_sdk_wrapper.hpp @@ -1,23 +1,24 @@ -/******************************************************************************* -* 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_ +// 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 @@ -29,9 +30,6 @@ #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 @@ -47,7 +45,7 @@ namespace turtlebot3 class DynamixelSDKWrapper { - public: +public: typedef struct { std::string usb_port; @@ -59,35 +57,34 @@ class DynamixelSDKWrapper explicit DynamixelSDKWrapper(const Device & device); virtual ~DynamixelSDKWrapper(); - template + template DataByteT get_data_from_device(const uint16_t & addr, const uint16_t & length) { DataByteT data = 0; - uint8_t * p_data = (uint8_t*)&data; + uint8_t * p_data = reinterpret_cast(&data); uint16_t index = addr - read_memory_.start_addr; std::lock_guard lock(read_data_mutex_); - switch (length) - { + switch (length) { case 1: - p_data[0] = read_memory_.data[index+0]; + 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]; + 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]; + 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; + p_data[0] = read_memory_.data[index + 0]; + break; } return data; @@ -104,7 +101,7 @@ class DynamixelSDKWrapper bool is_connected_to_device(); - private: +private: bool init_dynamixel_sdk_handlers(); bool read_register( @@ -142,6 +139,6 @@ class DynamixelSDKWrapper std::mutex read_data_mutex_; std::mutex write_data_mutex_; }; -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_DYNAMIXEL_SDK_WRAPPER_HPP_ +} // namespace turtlebot3 +} // namespace 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 index 1c580d95..1ed7cfd5 100644 --- a/turtlebot3_node/include/turtlebot3_node/odometry.hpp +++ b/turtlebot3_node/include/turtlebot3_node/odometry.hpp @@ -1,27 +1,21 @@ -/******************************************************************************* -* 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 +// 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 @@ -34,20 +28,25 @@ #include #include +#include +#include +#include +#include + namespace robotis { namespace turtlebot3 { class Odometry { - public: +public: explicit Odometry( std::shared_ptr & nh, const double wheels_separation, const double wheels_radius); - virtual ~Odometry(){}; + virtual ~Odometry() {} - private: +private: bool calculate_odometry(const rclcpp::Duration & duration); void update_imu(const std::shared_ptr & imu); @@ -72,8 +71,8 @@ class Odometry std::shared_ptr> msg_ftr_imu_sub_; typedef message_filters::sync_policies::ApproximateTime< - sensor_msgs::msg::JointState, - sensor_msgs::msg::Imu> SyncPolicyJointStateImu; + sensor_msgs::msg::JointState, + sensor_msgs::msg::Imu> SyncPolicyJointStateImu; typedef message_filters::Synchronizer SynchronizerJointStateImu; std::shared_ptr joint_state_imu_sync_; @@ -90,9 +89,9 @@ class Odometry std::array diff_joint_positions_; double imu_angle_; - std::array robot_pose_; - std::array robot_vel_; + std::array robot_pose_; + std::array robot_vel_; }; -} // turtlebot3 -} // robotis -#endif //TURTLEBOT3_NODE_ODOMETRY_HPP_ +} // namespace turtlebot3 +} // namespace 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 index 88090898..31eaa824 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/battery_state.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/battery_state.hpp @@ -1,26 +1,27 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ +// 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 -/* Author: Darby Lim */ - -#ifndef TURTLEBOT3_NODE_SENSORS_BATTERY_STATE_HPP_ -#define TURTLEBOT3_NODE_SENSORS_BATTERY_STATE_HPP_ +#ifndef TURTLEBOT3_NODE__SENSORS__BATTERY_STATE_HPP_ +#define TURTLEBOT3_NODE__SENSORS__BATTERY_STATE_HPP_ #include +#include +#include + #include "turtlebot3_node/sensors/sensors.hpp" namespace robotis @@ -31,7 +32,7 @@ namespace sensors { class BatteryState : public Sensors { - public: +public: explicit BatteryState( std::shared_ptr & nh, const std::string & topic_name = "battery_state"); @@ -40,10 +41,10 @@ class BatteryState : public Sensors const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) override; - private: +private: rclcpp::Publisher::SharedPtr pub_; }; -} // sensors -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_SENSORS_BATTERY_STATE_HPP_ +} // namespace sensors +} // namespace turtlebot3 +} // namespace 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 index a2c7d2ae..e0f988fb 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/imu.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/imu.hpp @@ -1,27 +1,28 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ +// 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 -/* Author: Darby Lim */ - -#ifndef TURTLEBOT3_NODE_SENSORS_IMU_HPP_ -#define TURTLEBOT3_NODE_SENSORS_IMU_HPP_ +#ifndef TURTLEBOT3_NODE__SENSORS__IMU_HPP_ +#define TURTLEBOT3_NODE__SENSORS__IMU_HPP_ #include #include +#include +#include + #include "turtlebot3_node/sensors/sensors.hpp" namespace robotis @@ -32,7 +33,7 @@ namespace sensors { class Imu : public Sensors { - public: +public: explicit Imu( std::shared_ptr & nh, const std::string & imu_topic_name = "imu", @@ -43,11 +44,11 @@ class Imu : public Sensors const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) override; - private: +private: rclcpp::Publisher::SharedPtr imu_pub_; rclcpp::Publisher::SharedPtr mag_pub_; }; -} // sensors -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_SENSORS_IMU_HPP_ +} // namespace sensors +} // namespace turtlebot3 +} // namespace 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 index a3a2c03c..a465d671 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/joint_state.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/joint_state.hpp @@ -1,26 +1,27 @@ -/******************************************************************************* -* 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_ +// 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 +#include + #include "turtlebot3_node/sensors/sensors.hpp" namespace robotis @@ -39,7 +40,7 @@ constexpr double TICK_TO_RAD = 0.001533981; class JointState : public Sensors { - public: +public: explicit JointState( std::shared_ptr & nh, const std::string & topic_name = "joint_states", @@ -49,10 +50,10 @@ class JointState : public Sensors const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) override; - private: +private: rclcpp::Publisher::SharedPtr pub_; }; -} // sensors -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_SENSORS_JOINT_STATE_HPP_ +} // namespace sensors +} // namespace turtlebot3 +} // namespace 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 index 229e9306..30b114f2 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/sensor_state.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/sensor_state.hpp @@ -1,26 +1,27 @@ -/******************************************************************************* -* 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_ +// 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 +#include + #include "turtlebot3_node/sensors/sensors.hpp" namespace robotis @@ -31,7 +32,7 @@ namespace sensors { class SensorState : public Sensors { - public: +public: explicit SensorState( std::shared_ptr & nh, const std::string & topic_name = "sensor_state", @@ -45,7 +46,7 @@ class SensorState : public Sensors const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) override; - private: +private: rclcpp::Publisher::SharedPtr pub_; bool bumper_forward_; @@ -54,7 +55,7 @@ class SensorState : public Sensors bool sonar_; bool illumination_; }; -} // sensors -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_SENSORS_SENSOR_STATE_HPP_ +} // namespace sensors +} // namespace turtlebot3 +} // namespace 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 index d1bd9c4d..c90faead 100644 --- a/turtlebot3_node/include/turtlebot3_node/sensors/sensors.hpp +++ b/turtlebot3_node/include/turtlebot3_node/sensors/sensors.hpp @@ -1,30 +1,28 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ +// 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_ -/* Author: Darby Lim */ - -#ifndef TURTLEBOT3_NODE_SENSORS_SENSORS_HPP_ -#define TURTLEBOT3_NODE_SENSORS_SENSORS_HPP_ +#include #include #include #include -#include - #include "turtlebot3_node/control_table.hpp" #include "turtlebot3_node/dynamixel_sdk_wrapper.hpp" @@ -37,7 +35,7 @@ namespace sensors { class Sensors { - public: +public: explicit Sensors( std::shared_ptr & nh, const std::string & frame_id = "") @@ -50,12 +48,12 @@ class Sensors const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) = 0; - protected: +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_ +} // namespace sensors +} // namespace turtlebot3 +} // namespace 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 index efefafb3..38ece2ea 100644 --- a/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp +++ b/turtlebot3_node/include/turtlebot3_node/turtlebot3.hpp @@ -1,31 +1,21 @@ -/******************************************************************************* -* 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 +// 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 @@ -36,6 +26,15 @@ #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" @@ -58,7 +57,7 @@ namespace turtlebot3 extern const ControlTable extern_control_table; class TurtleBot3 : public rclcpp::Node { - public: +public: typedef struct { float separation; @@ -72,12 +71,12 @@ class TurtleBot3 : public rclcpp::Node } Motors; explicit TurtleBot3(const std::string & usb_port); - virtual ~TurtleBot3(){}; + virtual ~TurtleBot3() {} Wheels * get_wheels(); Motors * get_motors(); - private: +private: void init_dynamixel_sdk_wrapper(const std::string & usb_port); void check_device_status(); @@ -99,8 +98,8 @@ class TurtleBot3 : public rclcpp::Node std::shared_ptr dxl_sdk_wrapper_; - std::list sensors_; - std::map devices_; + std::list sensors_; + std::map devices_; std::unique_ptr odom_; @@ -114,6 +113,6 @@ class TurtleBot3 : public rclcpp::Node rclcpp::AsyncParametersClient::SharedPtr priv_parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; }; -} // turtlebot3 -} // robotis -#endif // TURTLEBOT3_NODE_TURTLEBOT3_HPP_ +} // namespace turtlebot3 +} // namespace robotis +#endif // TURTLEBOT3_NODE__TURTLEBOT3_HPP_ diff --git a/turtlebot3_node/package.xml b/turtlebot3_node/package.xml index 5db04722..5a8d9aeb 100644 --- a/turtlebot3_node/package.xml +++ b/turtlebot3_node/package.xml @@ -5,15 +5,14 @@ 2.1.0 TurtleBot3 driver node that include diff drive controller, odometry and tf node - - Apache 2.0 - Darby Lim - Pyo + Will Son - http://wiki.ros.org/turtlebot3_node - http://turtlebot3.robotis.com + Apache 2.0 + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Darby Lim + Pyo ament_cmake dynamixel_sdk geometry_msgs diff --git a/turtlebot3_node/src/devices/motor_power.cpp b/turtlebot3_node/src/devices/motor_power.cpp index eeaa128b..abd0dce2 100644 --- a/turtlebot3_node/src/devices/motor_power.cpp +++ b/turtlebot3_node/src/devices/motor_power.cpp @@ -1,27 +1,27 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include + +using robotis::turtlebot3::devices::MotorPower; -devices::MotorPower::MotorPower( +MotorPower::MotorPower( std::shared_ptr & nh, std::shared_ptr & dxl_sdk_wrapper, const std::string & server_name) @@ -33,25 +33,25 @@ devices::MotorPower::MotorPower( [this]( const std::shared_ptr request, std::shared_ptr response) -> void - { - this->command(static_cast(request.get()), static_cast(response.get())); - } - ); + { + this->command(static_cast(request.get()), static_cast(response.get())); + } + ); } -void devices::MotorPower::command(const void * request, void * response) +void 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; + 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, + reinterpret_cast(&req.data), &res->message); } -void devices::MotorPower::request( +void MotorPower::request( rclcpp::Client::SharedPtr client, std_srvs::srv::SetBool::Request req) { diff --git a/turtlebot3_node/src/devices/reset.cpp b/turtlebot3_node/src/devices/reset.cpp index ddb0b5c5..76de1181 100644 --- a/turtlebot3_node/src/devices/reset.cpp +++ b/turtlebot3_node/src/devices/reset.cpp @@ -1,27 +1,27 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include + +using robotis::turtlebot3::devices::Reset; -devices::Reset::Reset( +Reset::Reset( std::shared_ptr & nh, std::shared_ptr & dxl_sdk_wrapper, const std::string & server_name) @@ -33,17 +33,17 @@ devices::Reset::Reset( [this]( const std::shared_ptr request, std::shared_ptr response) -> void - { - this->command(static_cast(request.get()), static_cast(response.get())); - } - ); + { + this->command(static_cast(request.get()), static_cast(response.get())); + } + ); } -void devices::Reset::command(const void * request, void * response) +void Reset::command(const void * request, void * response) { (void) request; - std_srvs::srv::Trigger::Response *res = (std_srvs::srv::Trigger::Response*)response; + std_srvs::srv::Trigger::Response * res = (std_srvs::srv::Trigger::Response *)response; uint8_t reset = 1; @@ -58,7 +58,7 @@ void devices::Reset::command(const void * request, void * response) RCLCPP_INFO(nh_->get_logger(), "Calibration End"); } -void devices::Reset::request( +void Reset::request( rclcpp::Client::SharedPtr client, std_srvs::srv::Trigger::Request req) { diff --git a/turtlebot3_node/src/devices/sound.cpp b/turtlebot3_node/src/devices/sound.cpp index b50139e1..95dd1126 100644 --- a/turtlebot3_node/src/devices/sound.cpp +++ b/turtlebot3_node/src/devices/sound.cpp @@ -1,27 +1,27 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include + +using robotis::turtlebot3::devices::Sound; -devices::Sound::Sound( +Sound::Sound( std::shared_ptr & nh, std::shared_ptr & dxl_sdk_wrapper, const std::string & server_name) @@ -33,25 +33,25 @@ devices::Sound::Sound( [this]( const std::shared_ptr request, std::shared_ptr response) -> void - { - this->command(static_cast(request.get()), static_cast(response.get())); - } - ); + { + this->command(static_cast(request.get()), static_cast(response.get())); + } + ); } -void devices::Sound::command(const void * request, void * response) +void 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; + 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, + reinterpret_cast(&req.value), &res->message); } -void devices::Sound::request( +void Sound::request( rclcpp::Client::SharedPtr client, turtlebot3_msgs::srv::Sound::Request req) { diff --git a/turtlebot3_node/src/diff_drive_controller.cpp b/turtlebot3_node/src/diff_drive_controller.cpp index 8a6743fc..488f7072 100644 --- a/turtlebot3_node/src/diff_drive_controller.cpp +++ b/turtlebot3_node/src/diff_drive_controller.cpp @@ -1,25 +1,24 @@ -/******************************************************************************* -* 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 */ +// 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; +#include + +using robotis::turtlebot3::DiffDriveController; DiffDriveController::DiffDriveController(const float wheel_seperation, const float wheel_radius) : Node("diff_drive_controller", rclcpp::NodeOptions().use_intra_process_comms(true)) diff --git a/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp b/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp index d4c34e97..1ec8e018 100644 --- a/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp +++ b/turtlebot3_node/src/dynamixel_sdk_wrapper.cpp @@ -1,35 +1,33 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include -DynamixelSDKWrapper::DynamixelSDKWrapper(const Device & device) : device_(device) +using robotis::turtlebot3::DynamixelSDKWrapper; + +DynamixelSDKWrapper::DynamixelSDKWrapper(const Device & device) +: device_(device) { - if (init_dynamixel_sdk_handlers() == false) - { + if (init_dynamixel_sdk_handlers() == false) { LOG_ERROR("DynamixelSDKWrapper", "Failed to initialize SDK handlers"); return; - } - else - { + } else { LOG_DEBUG("DynamixelSDKWrapper", "Success to initilize SDK handlers"); } } @@ -54,7 +52,7 @@ void DynamixelSDKWrapper::init_read_memory(const uint16_t & start_addr, const ui void DynamixelSDKWrapper::read_data_set() { - const char *log = NULL; + const char * log = NULL; bool ret = this->read_register( device_.id, read_memory_.start_addr, @@ -62,12 +60,9 @@ void DynamixelSDKWrapper::read_data_set() &read_data_buffer_[0], &log); - if (ret == false) - { + if (ret == false) { LOG_ERROR("DynamixelSDKWrapper", "Failed to read[%s]", log); - } - else - { + } 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"); @@ -80,19 +75,16 @@ bool DynamixelSDKWrapper::set_data_to_device( uint8_t * get_data, std::string * msg) { - const char *log = nullptr; + 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) - { + if (ret == true) { *msg = "Succeeded to write data"; return true; - } - else - { + } else { *msg = "Failed to write data" + std::string(log); return false; } @@ -103,24 +95,19 @@ bool DynamixelSDKWrapper::set_data_to_device( bool DynamixelSDKWrapper::init_dynamixel_sdk_handlers() { portHandler_ = dynamixel::PortHandler::getPortHandler(device_.usb_port.c_str()); - packetHandler_ = dynamixel::PacketHandler::getPacketHandler((int)device_.protocol_version); + packetHandler_ = + dynamixel::PacketHandler::getPacketHandler(static_cast(device_.protocol_version)); - if (portHandler_->openPort()) - { + if (portHandler_->openPort()) { LOG_INFO("DynamixelSDKWrapper", "Succeeded to open the port(%s)!", device_.usb_port.c_str()); - } - else - { + } else { LOG_ERROR("DynamixelSDKWrapper", "Failed to open the port(%s)!", device_.usb_port.c_str()); return false; } - if (portHandler_->setBaudRate((int)device_.baud_rate)) - { + if (portHandler_->setBaudRate(static_cast(device_.baud_rate))) { LOG_INFO("DynamixelSDKWrapper", "Succeeded to change the baudrate!"); - } - else - { + } else { LOG_ERROR("DynamixelSDKWrapper", "Failed to change the baudrate(%d)!", device_.baud_rate); return false; } @@ -148,18 +135,13 @@ bool DynamixelSDKWrapper::read_register( data_basket, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(dxl_comm_result); + 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); + } else if (dxl_error != 0) { + if (log != NULL) {*log = packetHandler_->getRxPacketError(dxl_error);} return false; - } - else - { + } else { return true; } @@ -186,18 +168,13 @@ bool DynamixelSDKWrapper::write_register( data, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - if (log != NULL) *log = packetHandler_->getTxRxResult(dxl_comm_result); + 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); + } else if (dxl_error != 0) { + if (log != NULL) {*log = packetHandler_->getRxPacketError(dxl_error);} return false; - } - else - { + } else { return true; } diff --git a/turtlebot3_node/src/node_main.cpp b/turtlebot3_node/src/node_main.cpp index de1c8932..d4f6e714 100644 --- a/turtlebot3_node/src/node_main.cpp +++ b/turtlebot3_node/src/node_main.cpp @@ -1,28 +1,26 @@ -/******************************************************************************* -* 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. -*******************************************************************************/ +// 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 -/* Author: Darby Lim */ +#include +#include #include #include #include -#include -#include - #include "turtlebot3_node/diff_drive_controller.hpp" #include "turtlebot3_node/turtlebot3.hpp" @@ -35,12 +33,11 @@ void help_print() printf("-i usb_port: Connected USB port with OpenCR."); } -int main(int argc, char *argv[]) +int main(int argc, char * argv[]) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - if (rcutils_cli_option_exist(argv, argv + argc, "-h")) - { + if (rcutils_cli_option_exist(argv, argv + argc, "-h")) { help_print(); return 0; } @@ -50,8 +47,7 @@ int main(int argc, char *argv[]) std::string usb_port = "/dev/ttyACM0"; char * cli_options; cli_options = rcutils_cli_get_option(argv, argv + argc, "-i"); - if (nullptr != cli_options) - { + if (nullptr != cli_options) { usb_port = std::string(cli_options); } @@ -60,8 +56,8 @@ int main(int argc, char *argv[]) auto turtlebot3 = std::make_shared(usb_port); auto diff_drive_controller = std::make_shared( - turtlebot3->get_wheels()->separation, - turtlebot3->get_wheels()->radius); + turtlebot3->get_wheels()->separation, + turtlebot3->get_wheels()->radius); executor.add_node(turtlebot3); executor.add_node(diff_drive_controller); diff --git a/turtlebot3_node/src/odometry.cpp b/turtlebot3_node/src/odometry.cpp index 898cb59f..378e854e 100644 --- a/turtlebot3_node/src/odometry.cpp +++ b/turtlebot3_node/src/odometry.cpp @@ -1,29 +1,30 @@ -/******************************************************************************* -* 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 */ +// 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/odometry.hpp" -using namespace robotis; +#include +#include +#include + +using robotis::turtlebot3::Odometry; using namespace std::chrono_literals; -using namespace turtlebot3; Odometry::Odometry( - std::shared_ptr &nh, + std::shared_ptr & nh, const double wheels_separation, const double wheels_radius) : nh_(nh), @@ -66,20 +67,19 @@ Odometry::Odometry( tf_broadcaster_ = std::make_unique(nh_); - if (use_imu_) - { + 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"); + nh_, + "joint_states"); msg_ftr_imu_sub_ = std::make_shared>( - nh_, - "imu"); + nh_, + "imu"); // connect message filters to synchronizer joint_state_imu_sync_->connectInput(*msg_ftr_joint_state_sub_, *msg_ftr_imu_sub_); @@ -92,14 +92,13 @@ Odometry::Odometry( 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_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, @@ -120,8 +119,8 @@ void Odometry::joint_state_callback(const sensor_msgs::msg::JointState::SharedPt } void Odometry::joint_state_and_imu_callback( - const std::shared_ptr &joint_state_msg, - const std::shared_ptr &imu_msg) + const std::shared_ptr & joint_state_msg, + const std::shared_ptr & imu_msg) { RCLCPP_DEBUG( nh_->get_logger(), @@ -145,7 +144,7 @@ void Odometry::publish(const rclcpp::Time & now) 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->child_frame_id = child_frame_id_of_odometry_; odom_msg->header.stamp = now; odom_msg->pose.pose.position.x = robot_pose_[0]; @@ -160,10 +159,10 @@ void Odometry::publish(const rclcpp::Time & now) 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.linear.x = robot_vel_[0]; odom_msg->twist.twist.angular.z = robot_vel_[2]; - // TODO: Find more accurate covariance. + // TODO(Will Son): 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; @@ -183,7 +182,7 @@ void Odometry::publish(const rclcpp::Time & now) 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.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_; @@ -191,12 +190,13 @@ void Odometry::publish(const rclcpp::Time & now) odom_pub_->publish(std::move(odom_msg)); - if (publish_tf_) + if (publish_tf_) { tf_broadcaster_->sendTransform(odom_tf); + } } void Odometry::update_joint_state( - const std::shared_ptr &joint_state) + const std::shared_ptr & joint_state) { static std::array last_joint_positions = {0.0f, 0.0f}; @@ -207,14 +207,14 @@ void Odometry::update_joint_state( last_joint_positions[1] = joint_state->position[1]; } -void Odometry::update_imu(const std::shared_ptr &imu) +void Odometry::update_imu(const std::shared_ptr & imu) { 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); + 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::calculate_odometry(const rclcpp::Duration &duration) +bool Odometry::calculate_odometry(const rclcpp::Duration & duration) { // rotation value of wheel [rad] double wheel_l = diff_joint_positions_[0]; @@ -233,24 +233,24 @@ bool Odometry::calculate_odometry(const rclcpp::Duration &duration) double step_time = duration.seconds(); - if (step_time == 0.0) + if (step_time == 0.0) { return false; + } - if (std::isnan(wheel_l)) + if (std::isnan(wheel_l)) { wheel_l = 0.0; + } - if (std::isnan(wheel_r)) + if (std::isnan(wheel_r)) { wheel_r = 0.0; + } delta_s = wheels_radius_ * (wheel_r + wheel_l) / 2.0; - if (use_imu_) - { + if (use_imu_) { theta = imu_angle_; delta_theta = theta - last_theta; - } - else - { + } else { theta = wheels_radius_ * (wheel_r - wheel_l) / wheels_separation_; delta_theta = theta; } diff --git a/turtlebot3_node/src/sensors/battery_state.cpp b/turtlebot3_node/src/sensors/battery_state.cpp index 07e1d091..705cf247 100644 --- a/turtlebot3_node/src/sensors/battery_state.cpp +++ b/turtlebot3_node/src/sensors/battery_state.cpp @@ -1,27 +1,28 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include +#include -sensors::BatteryState::BatteryState( +using robotis::turtlebot3::sensors::BatteryState; + +BatteryState::BatteryState( std::shared_ptr & nh, const std::string & topic_name) : Sensors(nh) @@ -31,7 +32,7 @@ sensors::BatteryState::BatteryState( RCLCPP_INFO(nh_->get_logger(), "Succeeded to create battery state publisher"); } -void sensors::BatteryState::publish( +void BatteryState::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { diff --git a/turtlebot3_node/src/sensors/imu.cpp b/turtlebot3_node/src/sensors/imu.cpp index 9fca607b..e6c69f21 100644 --- a/turtlebot3_node/src/sensors/imu.cpp +++ b/turtlebot3_node/src/sensors/imu.cpp @@ -1,27 +1,28 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include +#include -sensors::Imu::Imu( +using robotis::turtlebot3::sensors::Imu; + +Imu::Imu( std::shared_ptr & nh, const std::string & imu_topic_name, const std::string & mag_topic_name, @@ -34,7 +35,7 @@ sensors::Imu::Imu( RCLCPP_INFO(nh_->get_logger(), "Succeeded to create imu publisher"); } -void sensors::Imu::publish( +void Imu::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { diff --git a/turtlebot3_node/src/sensors/joint_state.cpp b/turtlebot3_node/src/sensors/joint_state.cpp index e7673ef7..a73b7e1e 100644 --- a/turtlebot3_node/src/sensors/joint_state.cpp +++ b/turtlebot3_node/src/sensors/joint_state.cpp @@ -1,29 +1,30 @@ -/******************************************************************************* -* 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 */ +// 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 +#include +#include + #include "turtlebot3_node/sensors/joint_state.hpp" -using namespace robotis; -using namespace turtlebot3; +using robotis::turtlebot3::sensors::JointState; -sensors::JointState::JointState( +JointState::JointState( std::shared_ptr & nh, const std::string & topic_name, const std::string & frame_id) @@ -34,7 +35,7 @@ sensors::JointState::JointState( RCLCPP_INFO(nh_->get_logger(), "Succeeded to create joint state publisher"); } -void sensors::JointState::publish( +void JointState::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { @@ -43,7 +44,7 @@ void sensors::JointState::publish( static std::array last_diff_position, last_position; std::array position = - {dxl_sdk_wrapper->get_data_from_device( + {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( @@ -51,7 +52,7 @@ void sensors::JointState::publish( extern_control_table.present_position_right.length)}; std::array velocity = - {dxl_sdk_wrapper->get_data_from_device( + {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( diff --git a/turtlebot3_node/src/sensors/sensor_state.cpp b/turtlebot3_node/src/sensors/sensor_state.cpp index bffcda4c..1ecb4427 100644 --- a/turtlebot3_node/src/sensors/sensor_state.cpp +++ b/turtlebot3_node/src/sensors/sensor_state.cpp @@ -1,27 +1,28 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include +#include -sensors::SensorState::SensorState( +using robotis::turtlebot3::sensors::SensorState; + +SensorState::SensorState( std::shared_ptr & nh, const std::string & topic_name, const bool & bumper_forward, @@ -41,7 +42,7 @@ sensors::SensorState::SensorState( RCLCPP_INFO(nh_->get_logger(), "Succeeded to create sensor state publisher"); } -void sensors::SensorState::publish( +void SensorState::publish( const rclcpp::Time & now, std::shared_ptr & dxl_sdk_wrapper) { @@ -49,8 +50,7 @@ void sensors::SensorState::publish( msg->header.stamp = now; - if (bumper_forward_ || bumper_backward_) - { + if (bumper_forward_ || bumper_backward_) { uint8_t bumper_push_state; uint8_t bumper_forward_state; uint8_t bumper_backward_state; @@ -63,78 +63,64 @@ void sensors::SensorState::publish( 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; + 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_) - { + } else if (!bumper_forward_ && !bumper_backward_) { msg->bumper = 0; } - if (cliff_) - { + if (cliff_) { msg->cliff = dxl_sdk_wrapper->get_data_from_device( extern_control_table.ir.addr, extern_control_table.ir.length); - } - else - { + } else { msg->cliff = 0.0f; } - if (sonar_) - { + if (sonar_) { msg->sonar = dxl_sdk_wrapper->get_data_from_device( extern_control_table.sonar.addr, extern_control_table.sonar.length); - } - else - { + } else { msg->sonar = 0.0f; } - if (illumination_) - { + if (illumination_) { msg->illumination = dxl_sdk_wrapper->get_data_from_device( extern_control_table.illumination.addr, extern_control_table.illumination.length); - } - else - { + } else { msg->illumination = 0.0f; } - if (sonar_) - { + if (sonar_) { msg->sonar = dxl_sdk_wrapper->get_data_from_device( extern_control_table.sonar.addr, extern_control_table.sonar.length); - } - else - { + } else { msg->sonar = 0.0f; } { - uint8_t button_push_state; - uint8_t button_0_state; - uint8_t button_1_state; + 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_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_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; + button_push_state = button_0_state << 0; + button_push_state |= button_1_state << 1; - msg->button = button_push_state; + msg->button = button_push_state; } msg->torque = dxl_sdk_wrapper->get_data_from_device( @@ -142,8 +128,8 @@ void sensors::SensorState::publish( 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); + 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, diff --git a/turtlebot3_node/src/turtlebot3.cpp b/turtlebot3_node/src/turtlebot3.cpp index f7148194..917aa036 100644 --- a/turtlebot3_node/src/turtlebot3.cpp +++ b/turtlebot3_node/src/turtlebot3.cpp @@ -1,29 +1,29 @@ -/******************************************************************************* -* 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 */ +// 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; +#include +#include + +using robotis::turtlebot3::TurtleBot3; 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)) +: 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 *) {}); @@ -39,12 +39,12 @@ TurtleBot3::TurtleBot3(const std::string & usb_port) run(); } -TurtleBot3::Wheels* TurtleBot3::get_wheels() +TurtleBot3::Wheels * TurtleBot3::get_wheels() { return &wheels_; } -TurtleBot3::Motors* TurtleBot3::get_motors() +TurtleBot3::Motors * TurtleBot3::get_motors() { return &motors_; } @@ -74,8 +74,7 @@ void TurtleBot3::init_dynamixel_sdk_wrapper(const std::string & usb_port) void TurtleBot3::check_device_status() { - if (dxl_sdk_wrapper_->is_connected_to_device()) - { + if (dxl_sdk_wrapper_->is_connected_to_device()) { std::string sdk_msg; uint8_t reset = 1; @@ -88,9 +87,7 @@ void TurtleBot3::check_device_status() RCLCPP_INFO(this->get_logger(), "Start Calibration of Gyro"); rclcpp::sleep_for(std::chrono::seconds(5)); RCLCPP_INFO(this->get_logger(), "Calibration End"); - } - else - { + } else { RCLCPP_ERROR(this->get_logger(), "Failed connection with Devices"); rclcpp::shutdown(); return; @@ -102,8 +99,7 @@ void TurtleBot3::check_device_status() extern_control_table.device_status.addr, extern_control_table.device_status.length); - switch (device_status) - { + switch (device_status) { case NOT_CONNECTED_MOTOR: RCLCPP_WARN(this->get_logger(), "Please double check your Dynamixels and Power"); break; @@ -164,24 +160,27 @@ void TurtleBot3::add_sensors() 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::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")); } @@ -213,17 +212,16 @@ void TurtleBot3::publish_timer(const std::chrono::milliseconds timeout) publish_timer_ = this->create_wall_timer( timeout, [this]() -> void - { - rclcpp::Time now = this->now(); + { + rclcpp::Time now = this->now(); - dxl_sdk_wrapper_->read_data_set(); + dxl_sdk_wrapper_->read_data_set(); - for (const auto &sensor:sensors_) - { - sensor->publish(now, dxl_sdk_wrapper_); - } + for (const auto & sensor : sensors_) { + sensor->publish(now, dxl_sdk_wrapper_); } - ); + } + ); } void TurtleBot3::heartbeat_timer(const std::chrono::milliseconds timeout) @@ -231,30 +229,28 @@ 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; + { + 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); + 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()); + RCLCPP_DEBUG(this->get_logger(), "hearbeat count : %d, msg : %s", count, msg.c_str()); - count++; - } - ); + 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()) - { + 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; } @@ -264,51 +260,48 @@ void TurtleBot3::parameter_event_callback() 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()); + 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; + 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 = + rclcpp::Parameter::from_parameter_msg(changed_parameter).as_double(); - motors_.profile_acceleration = - motors_.profile_acceleration / motors_.profile_acceleration_constant; + motors_.profile_acceleration = + motors_.profile_acceleration / motors_.profile_acceleration_constant; - union Data - { - int32_t dword[2]; - uint8_t byte[4*2]; - } data; + 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); + 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; + 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]; + uint8_t * p_data = &data.byte[0]; - dxl_sdk_wrapper_->set_data_to_device(start_addr, addr_length, p_data, &sdk_msg); + 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()); + 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); } @@ -320,35 +313,34 @@ void TurtleBot3::cmd_vel_callback() "cmd_vel", qos, [this](const geometry_msgs::msg::Twist::SharedPtr msg) -> void - { - std::string sdk_msg; + { + std::string sdk_msg; - union Data - { - int32_t dword[6]; - uint8_t byte[4*6]; - } data; + 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); + 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; + 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]; + uint8_t * p_data = &data.byte[0]; - dxl_sdk_wrapper_->set_data_to_device(start_addr, addr_length, p_data, &sdk_msg); + 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()); - } - ); + 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_teleop/package.xml b/turtlebot3_teleop/package.xml index 8441e452..48b7000f 100644 --- a/turtlebot3_teleop/package.xml +++ b/turtlebot3_teleop/package.xml @@ -6,14 +6,13 @@ Teleoperation node using keyboard for TurtleBot3. - Darby Lim - Pyo Will Son Apache 2.0 - http://wiki.ros.org/turtlebot3_teleop - http://turtlebot3.robotis.com + http://turtlebot3.robotis.com https://github.com/ROBOTIS-GIT/turtlebot3 https://github.com/ROBOTIS-GIT/turtlebot3/issues + Darby Lim + Pyo geometry_msgs rclpy diff --git a/turtlebot3_teleop/setup.py b/turtlebot3_teleop/setup.py index 4d4d8e7a..83d84e46 100644 --- a/turtlebot3_teleop/setup.py +++ b/turtlebot3_teleop/setup.py @@ -1,6 +1,3 @@ -import glob -import os - from setuptools import find_packages from setuptools import setup diff --git a/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py b/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py index 2ae60c35..b09285c9 100755 --- a/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py +++ b/turtlebot3_teleop/turtlebot3_teleop/script/teleop_keyboard.py @@ -1,44 +1,53 @@ +#!/usr/bin/env python +# # Copyright (c) 2011, Willow Garage, Inc. # All rights reserved. # +# Software License Agreement (BSD License 2.0) +# # Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: +# modification, are permitted provided that the following conditions +# are met: # -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. # -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. - +# # Author: Darby Lim import os import select import sys -if os.name == 'nt': - import msvcrt -else: - import termios, tty +import rclpy from geometry_msgs.msg import Twist -import rclpy from rclpy.qos import QoSProfile +if os.name == 'nt': + import msvcrt +else: + import termios + import tty + BURGER_MAX_LIN_VEL = 0.22 BURGER_MAX_ANG_VEL = 2.84 @@ -93,9 +102,9 @@ def print_vels(target_linear_velocity, target_angular_velocity): def make_simple_profile(output, input, slop): if input > output: - output = min( input, output + slop ) + output = min(input, output + slop) elif input < output: - output = max( input, output - slop ) + output = max(input, output - slop) else: output = input @@ -119,6 +128,7 @@ def check_linear_limit_velocity(velocity): else: return constrain(velocity, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL) + def check_angular_limit_velocity(velocity): if TURTLEBOT3_MODEL == 'burger': return constrain(velocity, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL) @@ -138,46 +148,46 @@ def main(): pub = node.create_publisher(Twist, 'cmd_vel', qos) status = 0 - target_linear_velocity = 0.0 - target_angular_velocity = 0.0 - control_linear_velocity = 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 = get_key(settings) - if key == 'w' : + if key == 'w': target_linear_velocity =\ check_linear_limit_velocity(target_linear_velocity + LIN_VEL_STEP_SIZE) status = status + 1 print_vels(target_linear_velocity, target_angular_velocity) - elif key == 'x' : + elif key == 'x': target_linear_velocity =\ check_linear_limit_velocity(target_linear_velocity - LIN_VEL_STEP_SIZE) status = status + 1 print_vels(target_linear_velocity, target_angular_velocity) - elif key == 'a' : + elif key == 'a': target_angular_velocity =\ check_angular_limit_velocity(target_angular_velocity + ANG_VEL_STEP_SIZE) status = status + 1 print_vels(target_linear_velocity, target_angular_velocity) - elif key == 'd' : + elif key == 'd': target_angular_velocity =\ check_angular_limit_velocity(target_angular_velocity - ANG_VEL_STEP_SIZE) status = status + 1 print_vels(target_linear_velocity, target_angular_velocity) - elif key == ' ' or key == 's' : - target_linear_velocity = 0.0 - control_linear_velocity = 0.0 - target_angular_velocity = 0.0 + elif key == ' ' or key == 's': + 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 - if status == 20 : + if status == 20: print(msg) status = 0 @@ -186,7 +196,7 @@ def main(): control_linear_velocity = make_simple_profile( control_linear_velocity, target_linear_velocity, - (LIN_VEL_STEP_SIZE/2.0)) + (LIN_VEL_STEP_SIZE / 2.0)) twist.linear.x = control_linear_velocity twist.linear.y = 0.0 @@ -195,7 +205,7 @@ def main(): control_angular_velocity = make_simple_profile( control_angular_velocity, target_angular_velocity, - (ANG_VEL_STEP_SIZE/2.0)) + (ANG_VEL_STEP_SIZE / 2.0)) twist.angular.x = 0.0 twist.angular.y = 0.0 @@ -221,5 +231,6 @@ def main(): if os.name != 'nt': termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + if __name__ == '__main__': main()