Feature/humble add gazebo

.gitignore

@@ -0,0 +1,2 @@
+
+build/

cube_petit_bringup/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(cube_petit_bringup)
+
+if(CMAKE_COMPILER_IS_GNUCXX
+   OR CMAKE_CXX_COMPILER_ID
+      MATCHES
+      "Clang"
+)
+  add_compile_options(
+    -Wall
+    -Wextra
+    -Wpedantic
+  )
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+install(
+  DIRECTORY launch config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
+install(
+  PROGRAMS scripts/twist_to_twist_stamped.py
+  DESTINATION lib/${PROJECT_NAME}
+)

cube_petit_bringup/config/ps4.config.yaml

@@ -0,0 +1,25 @@
+# Joy L Stick (0:Right)
+axis_linear: 1
+
+# ZENGO
+scale_linear: 0.3
+scale_linear_turbo: 0.7
+
+# KAITEN
+axis_angular: 0
+scale_angular: 5.0
+
+# TURBO
+enable_button: 1 # batu button
+enable_turbo_button: 6 # L1 shoulder button
+
+# 0:select  1:leftJOY   2:rightJOY  3:start
+# 4:ue      5:migi      6:sita      7:hidari
+# 10:L1      11:R1      8:L2        9:R2
+# 15:sikaku 14:batu,   13:maru,     12:sankaku
+# 16:ps
+
+# 0:sikaku, 1:batu,   2:maru,     3:sankaku
+# 4:L1      5:R1      6:L2        7:R2
+# 8:share   9:options 10:leftJOY  11:rightJOY
+# 12:ps     13: sikakuibotan

cube_petit_bringup/launch/teleop.launch.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+# -*- coding:utf-8 -*-
+
+# Copyright (c) 2024 SoftBank Corp.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate launch descriptions.
+
+    Returns:
+        Launch descriptions
+    """
+    use_sim_time_arg = DeclareLaunchArgument(
+        'use_sim_time',
+        default_value='false',
+        description='Use simulation (Gazebo) clock if true')
+    robot_namespace_arg = DeclareLaunchArgument(
+        'robot_namespace',
+        default_value='cube_petit')
+    cmd_vel_out_arg = DeclareLaunchArgument(
+        'cmd_vel_out',
+        default_value='/diff_drive_controller/cmd_vel_raw')
+    # default_value=[LaunchConfiguration('robot_namespace'), '/diff_drive_controller/cmd_vel_raw'])
+
+    container_name_arg = DeclareLaunchArgument(
+        'container_name', default_value='teleop_container',
+        description='the name of container that nodes will load in if use composition')
+
+    return LaunchDescription([
+        use_sim_time_arg,
+        robot_namespace_arg,
+        cmd_vel_out_arg,
+        container_name_arg,
+    ])

cube_petit_bringup/launch/teleop_key.launch.py

@@ -0,0 +1,58 @@
+#!/usr/bin/env python
+# -*- coding:utf-8 -*-
+
+# Copyright (c) 2024 SoftBank Corp.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description() -> LaunchDescription:
+    return LaunchDescription([
+        # Declare launch arguments
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='true',
+            description='Use simulation (Gazebo) clock if true'
+        ),
+        DeclareLaunchArgument(
+            'keyboard_topic',
+            default_value='/diff_drive_controller/cmd_vel',
+            description='Topic to publish keyboard teleop messages to'
+        ),
+
+        # Start teleop_twist_keyboard node
+        Node(
+            package='teleop_twist_keyboard',
+            executable='teleop_twist_keyboard',
+            name='teleop_twist_keyboard',
+            output='screen',
+            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
+            prefix='xterm -e'
+        ),
+        Node(
+            package='cube_petit_bringup',
+            executable='twist_to_twist_stamped.py',
+            name='twist_to_twist_stamped',
+            output='screen',
+            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}]
+        ),
+    ])
+
+
+if __name__ == '__main__':
+    generate_launch_description()

cube_petit_bringup/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<package format="3">
+  <name>cube_petit_bringup</name>
+  <version>2.2.0</version>
+  <description>The cube_petit_bringup package</description>
+  <maintainer email="[email protected]">SoftBank corp.</maintainer>
+  <license>Apache 2.0</license>
+  <url type="repository">https://github.com/sbgisen/cube_petit_ros</url>
+  <url type="bugtracker">https://github.com/sbgisen/cube_petit_ros/issues</url>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+  <exec_depend>nav2_lifecycle_manager</exec_depend>
+  <exec_depend>nav2_velocity_smoother</exec_depend>
+  <exec_depend>teleop_twist_keyboard</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
+  <exec_depend>xterm</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

cube_petit_bringup/scripts/twist_to_twist_stamped.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+
+# Copyright (c) 2024 SoftBank Corp.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import List
+
+import rclpy
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import TwistStamped
+from rclpy.node import Node
+from std_msgs.msg import Header
+
+
+class TwistToTwistStamped(Node):
+    def __init__(self) -> None:
+        super().__init__('twist_to_twist_stamped')
+        self.subscription = self.create_subscription(
+            Twist,
+            '/cmd_vel',
+            self.listener_callback,
+            10)
+        self.publisher_ = self.create_publisher(TwistStamped, '/diff_drive_controller/cmd_vel', 10)
+
+    def listener_callback(self, msg: Twist) -> None:
+        twist_stamped_msg = TwistStamped()
+        twist_stamped_msg.header = Header()
+        twist_stamped_msg.header.stamp = self.get_clock().now().to_msg()
+        twist_stamped_msg.header.frame_id = 'base_link'
+        twist_stamped_msg.twist = msg
+        self.publisher_.publish(twist_stamped_msg)
+
+
+def main(args: List[str] = None) -> None:
+    rclpy.init(args=args)
+    twist_to_twist_stamped = TwistToTwistStamped()
+    rclpy.spin(twist_to_twist_stamped)
+
+    twist_to_twist_stamped.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

cube_petit_bringup/setup.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+# -*- coding:utf-8 -*-
+
+# Copyright (c) 2024 SoftBank Corp.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+
+from setuptools import setup
+
+package_name = 'cube_petit_bringup'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[package_name],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='Your Name',
+    maintainer_email='[email protected]',
+    description='Twist to TwistStamped converter',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'twist_to_twist_stamped = cube_petit_bringup.twist_to_twist_stamped:main',
+        ],
+    },
+)

cube_petit_description/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(cube_petit_description)
+
+if(CMAKE_COMPILER_IS_GNUCXX
+   OR CMAKE_CXX_COMPILER_ID
+      MATCHES
+      "Clang"
+)
+  add_compile_options(
+    -Wall
+    -Wextra
+    -Wpedantic
+  )
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  meshes
+  rviz
+  xacro
+  launch
+  # ros2_control
+)

cube_petit_description/README.md

@@ -0,0 +1,7 @@
+## cube_petit_description PKG
+
+URDF and XACRO files for cube-petit
+
+cube-petitのURDFとXACROファイル置き場
+
+`ros2 launch cube_petit_description cube_petit_display.launch.py`