From 4e5daa879e29aa911d7f23672b5c530873c12c59 Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Mon, 27 Nov 2023 17:17:26 +0900
Subject: [PATCH 1/7] add analyzers

---
 .../config/cube_petit_analyzers.yaml          | 70 +++++++++++++++++++
 1 file changed, 70 insertions(+)
 create mode 100644 cube_petit_bringup/config/cube_petit_analyzers.yaml

diff --git a/cube_petit_bringup/config/cube_petit_analyzers.yaml b/cube_petit_bringup/config/cube_petit_analyzers.yaml
new file mode 100644
index 0000000..e96a92f
--- /dev/null
+++ b/cube_petit_bringup/config/cube_petit_analyzers.yaml
@@ -0,0 +1,70 @@
+analyzers:
+  battery:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: battery
+    analyzers:
+      epsolar_device:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: epsolar
+        find_and_remove_prefix: diagnostic_epsolar
+      battery:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: remaining_battery
+        find_and_remove_prefix: diagnostic_battery
+  motors:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: motors
+    analyzers:
+      wheel_right:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: wheel_right
+        find_and_remove_prefix: diagnostic_wheel_right_
+        num_items: 2
+      wheel_left:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: wheel_left
+        find_and_remove_prefix: diagnostic_wheel_left_
+        num_items: 2
+  sensors:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: sensors
+    analyzers:
+      map_scan:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: map_scan
+        find_and_remove_prefix: diagnostic_map_scan_
+      depth_camera:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: depth_camera
+        find_and_remove_prefix: diagnostic_depth_camera_
+        num_items: 2
+      imu:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: imu
+        find_and_remove_prefix: diagnostic_imu_
+        num_items: 2
+      foot_scan:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: foot_scan
+        find_and_remove_prefix: diagnostic_foot_scan_
+        num_items: 2
+      body_scan:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: body_scan
+        find_and_remove_prefix: diagnostic_body_scan_
+        num_items: 2
+      head_scan:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: head_scan
+        find_and_remove_prefix: diagnostic_head_scan_
+        num_items: 2
+  move_base:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: move_base
+    analyzers:
+      movebase:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: status
+        find_and_remove_prefix: diagnostic_move_base_status
+        discard_stale: true
+        timeout: 5.0

From ae75fb27fa01ace2ec994f23da222d664b9c3f5b Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Mon, 27 Nov 2023 17:17:42 +0900
Subject: [PATCH 2/7] add imu

---
 cube_petit_bringup/config/imu_witmotion.yaml  | 69 +++++++++++++++++++
 .../config/sensors/imu_filter.yaml            |  9 +++
 cube_petit_bringup/launch/imu.launch          | 26 +++++++
 .../launch/includes/imu_filters.launch        | 21 ++++++
 4 files changed, 125 insertions(+)
 create mode 100644 cube_petit_bringup/config/imu_witmotion.yaml
 create mode 100644 cube_petit_bringup/config/sensors/imu_filter.yaml
 create mode 100644 cube_petit_bringup/launch/imu.launch
 create mode 100644 cube_petit_bringup/launch/includes/imu_filters.launch

diff --git a/cube_petit_bringup/config/imu_witmotion.yaml b/cube_petit_bringup/config/imu_witmotion.yaml
new file mode 100644
index 0000000..3ca9d63
--- /dev/null
+++ b/cube_petit_bringup/config/imu_witmotion.yaml
@@ -0,0 +1,69 @@
+witmotion_imu:
+    port: ttyIMU
+    baud_rate: 9600 # baud
+    polling_interval: 50 # ms
+    restart_service_name: /restart_imu
+    imu_publisher:
+        topic_name: /gyro/imu_raw
+        frame_id: gyro_link
+        use_native_orientation: false
+        measurements:
+            acceleration:
+                enabled: true
+                covariance: [ 0.0088,    0,       0,
+                              0,         0.0550,  0,
+                              0,         0,       0.0267 ]
+            angular_velocity:
+                enabled: true
+                covariance: [ 0.1199,    0,       0,
+                              0,         0.5753,  0,
+                              0,         0,       0.0267 ]
+            orientation:
+                enabled: true
+                covariance: [ 0.0190,    0,       0,
+                              0,         0.0120,  0,
+                              0,         0,       0.0107 ]
+    temperature_publisher:
+        enabled: true
+        topic_name: /temperature
+        frame_id: base_link
+        from_message: magnetometer # acceleration, angular_vel, orientation, magnetometer
+        variance: 0.02683
+        coefficient: 1.0 # Linear calibration parameters: coefficient
+        addition: 0.0 # and addendum
+    magnetometer_publisher:
+        enabled: true
+        topic_name: /magnetometer
+        frame_id: imu
+        coefficient: 0.00000001 # Linear calibration parameters: coefficient
+        addition: 0.0 # and addendum
+        covariance: [ 0.000000187123,   0,              0,
+                      0,                0.000000105373, 0,
+                      0,                0,              0.000000165816  ]
+    barometer_publisher:
+        enabled: true
+        topic_name: /barometer
+        frame_id: base_link
+        variance: 0.001
+        coefficient: 1.0 # Linear calibration parameters: coefficient
+        addition: 0.0 # and addendum
+    altimeter_publisher:
+        enabled: true
+        topic_name: /altitude
+        coefficient: 1.0 # Linear calibration parameters: coefficient
+        addition: 0.0 # and addendum
+    orientation_publisher:
+        enabled: true
+        topic_name: /orientation
+    gps_publisher:
+        enabled: false
+        navsat_fix_frame_id: world
+        navsat_fix_topic_name: /gps
+        navsat_altitude_topic_name: /gps_altitude
+        navsat_satellites_topic_name: /gps_satellites
+        navsat_variance_topic_name: /gps_variance
+        ground_speed_topic_name: /gps_ground_speed
+    rtc_publisher:
+        enabled: true
+        topic_name: /witmotion_clock
+        presync: true
diff --git a/cube_petit_bringup/config/sensors/imu_filter.yaml b/cube_petit_bringup/config/sensors/imu_filter.yaml
new file mode 100644
index 0000000..4094004
--- /dev/null
+++ b/cube_petit_bringup/config/sensors/imu_filter.yaml
@@ -0,0 +1,9 @@
+publish_debug_topics: false
+yaw_only_mode: true
+drift_wait_time: 4.0
+drift_lpf_time_constant: 3.96
+thresh_imu_gyro_stop: 0.5
+thresh_imu_jerk_stop: 20.0
+thresh_odom_twist_stop: 0.2
+thresh_cmd_vel_stop: 0.01
+cmd_vel_timeout: 3.0
diff --git a/cube_petit_bringup/launch/imu.launch b/cube_petit_bringup/launch/imu.launch
new file mode 100644
index 0000000..f1269be
--- /dev/null
+++ b/cube_petit_bringup/launch/imu.launch
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="port" default="/dev/ttyIMU"/>
+  <arg name="frame" default="gyro_link"/>
+  <arg name="topic" default="/gyro/imu"/>
+  <arg name="robot_namespace" default="cube_petit"/>
+  <!-- IMU node -->
+  <rosparam command="load" file="$(find cube_petit_bringup)/config/imu_witmotion.yaml" />
+  <node name="witmotion_imu" pkg="witmotion_ros"  type="witmotion_ros_node" output="screen"/>
+  <node name="imu_republish" pkg="topic_tools" type="drop" args="$(arg topic)_raw 1 2 $(arg topic)"/>
+  <!-- IMU filter -->
+  <include file="$(find cube_petit_bringup)/launch/includes/imu_filters.launch">
+    <arg name="robot_namespace" value="$(arg robot_namespace)"/>
+    <arg name="topic" value="$(arg topic)"/>
+  </include>
+  <!-- IMU diagnostic -->
+  <node pkg="cube_diagnostic" type="diagnostic_device.py" name="diagnostic_imu_device" output="log">
+    <param name="devicename" value="$(arg port)"/>
+  </node>
+  <node pkg="cube_diagnostic" type="diagnostic_imu.py" name="diagnostic_imu_topic" output="log">
+    <param name="id" value="imu"/>
+    <param name="topic" value="$(arg topic)"/>
+    <param name="min_publish_rate" value="1"/>
+    <param name="max_publish_rate" value="100"/>
+  </node>
+</launch>
diff --git a/cube_petit_bringup/launch/includes/imu_filters.launch b/cube_petit_bringup/launch/includes/imu_filters.launch
new file mode 100644
index 0000000..f583726
--- /dev/null
+++ b/cube_petit_bringup/launch/includes/imu_filters.launch
@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="topic" default="/gyro/imu"/>
+  <arg name="robot_namespace" default="cube_petit"/>
+  <!-- IMU filter -->
+  <node name="imu_filter" pkg="imu_drift_correction" type="imu_drift_correction" output="screen">
+    <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/imu_filter.yaml"/>
+    <remap from="~imu_in" to="$(arg topic)"/>
+    <remap from="~cmd_vel_in" to="/cube_petit/diff_drive_controller/cmd_vel"/>
+    <remap from="~odom_in" to="odom"/>
+    <remap from="~imu_out" to="/imu/data_raw"/>
+  </node>
+  <node name="imu_filter_madgwick" pkg="imu_filter_madgwick" type="imu_filter_node">
+    <param name="use_mag" value="false"/>
+    <param name="publish_tf" value="false"/>
+    <param name="fixed_frame" value="odom"/>
+    <param name="publish_debug_topics" value="true"/>
+    <param name="orientation_stddev" value="0.0316227766"/>
+    <remap from="/imu/data" to="/cube_petit/diff_drive_controller/imu"/>
+  </node>
+</launch>

From 6831518f5ec6432d025bb2b2ff4ce4ab3878ffbc Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Mon, 27 Nov 2023 17:19:41 +0900
Subject: [PATCH 3/7] update yaml and launch

---
 cube_petit_bringup/config/ps3.config.yaml     | 15 ++++++---
 .../sensors/cube_petit_pacecat_filter2.yaml   | 17 ++++++----
 .../launch/cube_petit_bringup.launch          | 33 +++++++++++--------
 .../launch/rear_camera_bringup.launch         | 22 ++++++++-----
 .../launch/talk_and_listen.launch             | 14 ++++++++
 5 files changed, 67 insertions(+), 34 deletions(-)
 create mode 100644 cube_petit_bringup/launch/talk_and_listen.launch

diff --git a/cube_petit_bringup/config/ps3.config.yaml b/cube_petit_bringup/config/ps3.config.yaml
index d2e8ba4..65f079c 100644
--- a/cube_petit_bringup/config/ps3.config.yaml
+++ b/cube_petit_bringup/config/ps3.config.yaml
@@ -1,15 +1,20 @@
+# Joy L Stick (0:Right)
 axis_linear: 1
-scale_linear: 0.5
-scale_linear_turbo: 1.5
 
+# ZENGO
+scale_linear: 0.3
+scale_linear_turbo: 0.7
+
+# KAITEN
 axis_angular: 0
 scale_angular: 5.0
 
-enable_button: 1  # batu button
-enable_turbo_button: 10  # L1 shoulder button
+# 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
\ No newline at end of file
+# 16:ps
diff --git a/cube_petit_bringup/config/sensors/cube_petit_pacecat_filter2.yaml b/cube_petit_bringup/config/sensors/cube_petit_pacecat_filter2.yaml
index 10e9d3e..6c5c83b 100644
--- a/cube_petit_bringup/config/sensors/cube_petit_pacecat_filter2.yaml
+++ b/cube_petit_bringup/config/sensors/cube_petit_pacecat_filter2.yaml
@@ -1,11 +1,16 @@
 scan_filter_chain:
-- name: angle0    # left side
+- name: angle0    # right front
   type: laser_filters/LaserScanAngularBoundsFilterInPlace
   params:
-    lower_angle:  -1.5708
-    upper_angle:  -0.6098
-- name: angle1   # right side
+    lower_angle:  -1.2217
+    upper_angle:  -0.5236
+- name: angle1   # left behind
   type: laser_filters/LaserScanAngularBoundsFilterInPlace
   params:
-    lower_angle: 1.5708
-    upper_angle: 2.5418
\ No newline at end of file
+    lower_angle: 1.9199
+    upper_angle: 2.6180
+
+# with zero angle being forward along the x axis
+# COUNTERclockwise (right+, left-)
+# range is - 3.1415 to 3.1415 [rad]
+# 30deg = 0.5236 70deg = 1.2217 110de = 1.0199 120de = 2.0944 150de = 2.6180
diff --git a/cube_petit_bringup/launch/cube_petit_bringup.launch b/cube_petit_bringup/launch/cube_petit_bringup.launch
index 0491c3b..53ed618 100644
--- a/cube_petit_bringup/launch/cube_petit_bringup.launch
+++ b/cube_petit_bringup/launch/cube_petit_bringup.launch
@@ -8,24 +8,30 @@
   <include file="$(find cube_petit_bringup)/launch/teleop_cube_petit.launch"/>
   <!-- <group ns="cube_petit"> -->
   <include file="$(find cube_petit_hardware_interface)/launch/motor_bringup.launch"/>
-    <!-- <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> -->
-  <!-- </group> -->
+  <node name="robot_state_publisher2" pkg="robot_state_publisher" type="robot_state_publisher"/>
+  <!-- launch teleop -->
+  <include file="$(find cube_petit_bringup)/launch/teleop_cube_petit.launch"/>
 
+  <!-- launch LiDAR -->
   <!-- laser filter node ( cut data of cube-petit's pole) -->
   <group ns="/pacecat">
     <include file="$(find cube_petit_bringup)/launch/lidar_bringup.launch"/>
     <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="cube_petit_pacecat_filter">
-      <!-- <remap from="scan" to="scan" /> -->
-      <!-- <remap from="scan_filtered" to="scan_filtered"/> -->
-      <!-- v1 -->
-      <!-- <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" /> -->
-      <!-- v2 -->
       <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter2.yaml"/>
     </node>
   </group>
- 
-  <!-- <include file="$(find cube_petit_bringup)/launch/realsense_rgbd.launch"/> -->
 
+  <!-- launch IMU -->
+  <!-- <include file="$(find wt901B_ros)/launch/wt901B_ros.launch"/> -->
+  <!-- <remap from="scan" to="scan" /> -->
+  <!-- <remap from="scan_filtered" to="scan_filtered"/> -->
+  <!-- v1 -->
+  <!-- <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" /> -->
+  <!-- v2 -->
+  <!-- imaha iranai -->
+  <include file="$(find cube_petit_bringup)/launch/realsense_rgbd.launch"/>
+
+  <!-- launch CAMERA -->
   <include file="$(find cube_petit_bringup)/launch/rear_camera_bringup.launch"/>
 
   <include file="$(find jbd_battery_monitor)/launch/battery_monitor.launch"/>
@@ -36,12 +42,11 @@
 
   <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch">
     <arg name="port" value="9090"/>
-    <arg name="websocket_ping_interval" value="5" />
-    <arg name="websocket_ping_timeout" value="3" />
+    <arg name="websocket_ping_interval" value="5"/>
+    <arg name="websocket_ping_timeout" value="3"/>
   </include>
-
-  <include file="$(find cube_diagnostic)/launch/diagnostic.launch">
-      <arg name="config_file" value="cube_petit_analyzers.yaml"/>
+  <include file="$(find cube_diagnostic)/launch/diagnostic_aggregator.launch">
+    <arg name="config_file" value="cube_petit_analyzers.yaml"/>
   </include>
 
 </launch>
\ No newline at end of file
diff --git a/cube_petit_bringup/launch/rear_camera_bringup.launch b/cube_petit_bringup/launch/rear_camera_bringup.launch
index d855ffa..b014a80 100644
--- a/cube_petit_bringup/launch/rear_camera_bringup.launch
+++ b/cube_petit_bringup/launch/rear_camera_bringup.launch
@@ -3,30 +3,34 @@
 
   <group ns="/rear_camera/rear_camera">
     <!-- launch カメラ -->>
-    <node pkg="uvc_camera" type="uvc_camera_node" name="usb_camera_node">
-        <param name="device" type="string" value="/dev/video0"/>
+    <!-- <node pkg="uvc_camera" type="uvc_camera_node" name="usb_camera_node">
+        <param name="device" type="string" value="/dev/rear_camera"/>
         <param name="pixel_format" value="YUYV"/>
         <param name="camera_frame_id" value="rear_camera"/>
         <param name="io_method" value="mmap"/>
         <param name="frame_id" value="rear_camera_optical_link"/>
+        <param name="camera_info_url" type="string" value="file://$(find cube_petit_bringup)/config/sensors/rear_camera/camera.yaml"/> </node>  -->
+    <node pkg="usb_cam" type="usb_cam_node" name="usb_cam">
+        <param name="device" type="string" value="/dev/rear_camera"/>
+        <param name="pixel_format" value="yuyv"/>
+        <param name="camera_frame_id" value="rear_camera"/>
+        <param name="io_method" value="mmap"/>
+        <param name="frame_id" value="rear_camera_optical_link"/>
         <param name="camera_info_url" type="string" value="file://$(find cube_petit_bringup)/config/sensors/rear_camera/camera.yaml"/> -->
-    </node> 
-    <node name="image_raw_view" pkg="image_view" type="image_view" >
-      <remap from="image" to="image_raw"/>
     </node>
   </group>
 
     <!-- ARマーカー Settings --><!-- marker size [cm]-->
-    <!-- <arg name="marker_size" default="5.5" /> 
+    <!-- <arg name="marker_size" default="5.5" />
     <arg name="max_new_marker_error" default="0.08" />
     <arg name="max_track_error" default="0.2" />
     <arg name="cam_image_topic" default="/rear_camera/image_raw" />
     <arg name="cam_info_topic" default="/rear_camera/camera_info" />
     <arg name="output_frame" default="/rear_cameras" /> -->
-    
+
     <!-- launch AR_TRACK_ALVAR -->
     <!-- <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen"
-    args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) 
+    args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error)
           $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" /> -->
 
 
@@ -64,7 +68,7 @@ Format Video Capture:
 	Transfer Function : Default
 	YCbCr Encoding    : Default
 	Quantization      : Default
-	Flags             : 
+	Flags             :
 Crop Capability Video Capture:
 	Bounds      : Left 0, Top 0, Width 640, Height 480
 	Default     : Left 0, Top 0, Width 640, Height 480
diff --git a/cube_petit_bringup/launch/talk_and_listen.launch b/cube_petit_bringup/launch/talk_and_listen.launch
new file mode 100644
index 0000000..8eecd96
--- /dev/null
+++ b/cube_petit_bringup/launch/talk_and_listen.launch
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- FOR PS4 TALK TEST -->
+  <!-- <include file="$(find cube_petit_bringup)/launch/teleop_cube_petit.launch"/> -->
+
+  <include file="$(find cube_speech)/launch/speech_server.launch"/>
+  <include file="$(find cube_petit_text_to_speech)/launch/cube_petit_text_to_speech.launch"/>
+  <include file="$(find cube_petit_speech_to_text)/launch/cube_petit_speech_to_text.launch"/>
+  <include file="$(find speech_recognition_ros)/launch/efficientword_net_hotword_detector.launch"/>
+
+<!-- ハローワールド！5月14日にロスロボットパーティーに参加します！もっと可愛くなった僕と遊んでね！ -->
+
+</launch>

From 372aaa7da06e58691e971017df9fe6d1eb0a3934 Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Mon, 27 Nov 2023 17:20:10 +0900
Subject: [PATCH 4/7] add mesh

---
 .../meshes/cube_petit/wheels/tamiya_wheel.mtl       |   0
 .../meshes/cube_petit/wheels/tamiya_wheel.obj       |   0
 .../meshes/cube_petit/wheels/tamiya_wheel.stl       | Bin
 .../meshes/cube_petit/wheels/tamiya_wheel.txt       |   0
 4 files changed, 0 insertions(+), 0 deletions(-)
 mode change 100644 => 100755 cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.mtl
 mode change 100644 => 100755 cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.obj
 mode change 100644 => 100755 cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.stl
 mode change 100644 => 100755 cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.txt

diff --git a/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.mtl b/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.mtl
old mode 100644
new mode 100755
diff --git a/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.obj b/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.obj
old mode 100644
new mode 100755
diff --git a/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.stl b/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.stl
old mode 100644
new mode 100755
diff --git a/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.txt b/cube_petit_description/meshes/cube_petit/wheels/tamiya_wheel.txt
old mode 100644
new mode 100755

From ecc233ea5a1a3e2fd1199b9c68456de646b6a665 Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Mon, 27 Nov 2023 17:20:17 +0900
Subject: [PATCH 5/7] fix lidar

---
 cube_petit_description/urdf/sensors/lidar.xacro | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/cube_petit_description/urdf/sensors/lidar.xacro b/cube_petit_description/urdf/sensors/lidar.xacro
index 4c395a5..f292ec5 100644
--- a/cube_petit_description/urdf/sensors/lidar.xacro
+++ b/cube_petit_description/urdf/sensors/lidar.xacro
@@ -3,7 +3,7 @@
   <joint name="laser_joint" type="fixed">
     <parent link="body_link"/>
     <child link="pacecat_link"/>
-    <origin rpy="0 0 3.14159" xyz="0.0 0 0.23975"/>
+    <origin rpy="0 0 0" xyz="0.0 0 0.23975"/>
   </joint>
   <link name="pacecat_link">
     <inertial>
@@ -63,4 +63,3 @@
     </sensor>
   </gazebo>
 </robot>
-

From 8646ddc30d7c2879d44fcf6b44032ff6050135bc Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Mon, 27 Nov 2023 17:24:25 +0900
Subject: [PATCH 6/7] update gazebo launch

---
 .../launch/cube_petit_gazebo_pc.launch        | 130 ++++++++----------
 1 file changed, 56 insertions(+), 74 deletions(-)

diff --git a/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch b/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch
index b7521c1..d70f69b 100644
--- a/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch
+++ b/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch
@@ -1,69 +1,53 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-
-    <!-- load cube_petit urdf -->
-    <arg name="cube_petit_version1" default="true"/>
-    <arg name="cube_petit_version2" default="false"/>
-
-
-    <!-- version1 -->
-    <!-- <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit.xacro" /> -->
-    <!-- <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo.yaml"/> -->
-
-    <!-- version2 -->
-    <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit_v2.xacro" />
-    <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo2.yaml"/>
-
-    <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>
-
-    <env name="GAZEBO_MODEL_PATH" value="$(find cube_petit_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)" />
-
-    <!-- cut laser data of cube_petit's pole -->
-    <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="cube_petit_pacecat_filter" ns="/pacecat">
-      <remap from="scan" to="scan" />
-      <remap from="scan_filtered" to="scan_filtered" />
-      <!-- v1 -->
-      <!-- <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" /> -->
-      <!-- v2 -->
-      <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter2.yaml" />
-    </node>
-
-    <arg name="model2" default="$(find cube_petit_gazebo)/models/charge_doc/cube_petit_charge_doc.xacro"/>   
-    <param name="robot_description2" command="$(find xacro)/xacro $(arg model2) --inorder"/>
-
-    <!-- launch gazebo -->
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <arg name="paused" value="false"/>
-        <arg name="use_sim_time" value="true"/>
-        <arg name="gui" value="true"/>
-        <arg name="headless" value="false"/>
-        <arg name="debug" value="false"/>
-        <!-- load world file from /worlds/ -->
-        <arg name="world_name" value="$(find cube_petit_gazebo)/worlds/cube_with_house.world"/>
-    </include>
-
-    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model cube_petit -x 1 -y -2.5" />
-    <node name="spawn_urdf2" pkg="gazebo_ros" type="spawn_model" args="-param robot_description2 -urdf -model cube_petit_charge_doc -x 0 -y -2.5" />
-
-
-    <!-- load controller_manager and robot_state_publisher -->
-    <param name="/diff_drive_controller/enable_odom_tf" value="true"/>
-    <!-- for gazebo param file is /config/odom_gazebo.yaml -->
-
-
-    <node name="controller" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/cube_petit" args="joint_state_controller diff_drive_controller"/>
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" ns="/cube_petit"/>
-    
-    <!-- launch rviz -->
-    <node name="rviz" pkg="rviz" type="rviz" args="" required="false"/>
-    <group ns="rviz_topics_relay">
-      <node name="clicked_point_relay" pkg="topic_tools" type="relay" args="/clicked_point /navigation/clicked_point"/>
-      <node name="initialpose_relay" pkg="topic_tools" type="relay" args="/initialpose /navigation/initialpose"/>
-      <node name="simple_goal_relay" pkg="topic_tools" type="relay" args="/move_base_simple/goal /navigation/move_base_simple/goal"/>
-    </group>
-
-    <!-- launch teleop -->
-    <!-- <group ns="joystick">
+  <!-- load cube_petit urdf -->
+  <arg name="cube_petit_version1" default="true"/>
+  <arg name="cube_petit_version2" default="false"/>
+  <!-- version1 -->
+  <!-- <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit.xacro" /> -->
+  <!-- <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo.yaml"/> -->
+  <!-- version2 -->
+  <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit_v2.xacro"/>
+  <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo2.yaml"/>
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>
+  <env name="GAZEBO_MODEL_PATH" value="$(find cube_petit_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
+  <!-- cut laser data of cube_petit's pole -->
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="cube_petit_pacecat_filter" ns="/pacecat">
+    <remap from="scan" to="scan"/>
+    <remap from="scan_filtered" to="scan_filtered"/>
+    <!-- v1 -->
+    <!-- <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" /> -->
+    <!-- v2 -->
+    <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter2.yaml"/>
+  </node>
+  <arg name="model2" default="$(find cube_petit_gazebo)/models/charge_doc/cube_petit_charge_doc.xacro"/>
+  <param name="robot_description2" command="$(find xacro)/xacro $(arg model2) --inorder"/>
+  <!-- launch gazebo -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="paused" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+    <arg name="gui" value="true"/>
+    <arg name="headless" value="false"/>
+    <arg name="debug" value="false"/>
+    <!-- load world file from /worlds/ -->
+    <arg name="world_name" value="$(find cube_petit_gazebo)/worlds/cube_with_house.world"/>
+  </include>
+  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model cube_petit -x 1 -y -2.5"/>
+  <node name="spawn_urdf2" pkg="gazebo_ros" type="spawn_model" args="-param robot_description2 -urdf -model cube_petit_charge_doc -x 0 -y -2.5"/>
+  <!-- load controller_manager and robot_state_publisher -->
+  <param name="/diff_drive_controller/enable_odom_tf" value="true"/>
+  <!-- for gazebo param file is /config/odom_gazebo.yaml -->
+  <node name="controller" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/cube_petit" args="joint_state_controller diff_drive_controller"/>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" ns="/cube_petit"/>
+  <!-- launch rviz -->
+  <node name="rviz" pkg="rviz" type="rviz" args="" required="false"/>
+  <group ns="rviz_topics_relay">
+    <node name="clicked_point_relay" pkg="topic_tools" type="relay" args="/clicked_point /clicked_point"/>
+    <node name="initialpose_relay" pkg="topic_tools" type="relay" args="/initialpose /initialpose"/>
+    <node name="simple_goal_relay" pkg="topic_tools" type="relay" args="/move_base_simple/goal /move_base_simple/goal"/>
+  </group>
+  <!-- launch teleop -->
+  <!-- <group ns="joystick">
       <arg name="teleop_config" default="$(find cube_petit_bringup)/config/ps3.config.yaml"/>
       <node name="joy_node" pkg="joy" type="joy_node"/>
       <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
@@ -74,19 +58,17 @@
      -->
   <group ns="joystick">
     <arg name="teleop_config" default="$(find cube_petit_bringup)/config/ps3.config.yaml"/>
-    <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder" />
-    <node name="joy_node" pkg="joy" type="joy_node" >
+    <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>
+    <node name="joy_node" pkg="joy" type="joy_node">
       <param name="autorepeat_rate" value="10.0"/>
     </node>
     <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
-      <rosparam command="load" file="$(arg teleop_config)" />
+      <rosparam command="load" file="$(arg teleop_config)"/>
       <remap from="cmd_vel" to="/cube_petit/diff_drive_controller/cmd_vel"/>
     </node>
-   </group>
-
-       <!-- Charging simulation -->
-    <include file="$(find cube_petit_gazebo)/launch/include/docking_station_simulation.launch">
-      <arg name="robot" value="cube-petit"/>
-    </include>
-
+  </group>
+  <!-- Charging simulation -->
+  <include file="$(find cube_petit_gazebo)/launch/include/docking_station_simulation.launch">
+    <arg name="robot" value="cube-petit"/>
+  </include>
 </launch>
\ No newline at end of file

From 7fb146ad0c6570536b1e181084594fb0f477b6b0 Mon Sep 17 00:00:00 2001
From: AYokochi <airi.yokochi@g.softbank.co.jp>
Date: Tue, 4 Jun 2024 11:00:07 +0900
Subject: [PATCH 7/7] fix launch file

---
 .../launch/cube_petit_bringup.launch          | 10 +-----
 .../launch/rear_camera_bringup.launch         | 24 -------------
 .../launch/cube_petit_gazebo_pc.launch        | 34 +++++++++----------
 3 files changed, 17 insertions(+), 51 deletions(-)

diff --git a/cube_petit_bringup/launch/cube_petit_bringup.launch b/cube_petit_bringup/launch/cube_petit_bringup.launch
index 53ed618..4704103 100644
--- a/cube_petit_bringup/launch/cube_petit_bringup.launch
+++ b/cube_petit_bringup/launch/cube_petit_bringup.launch
@@ -21,14 +21,6 @@
     </node>
   </group>
 
-  <!-- launch IMU -->
-  <!-- <include file="$(find wt901B_ros)/launch/wt901B_ros.launch"/> -->
-  <!-- <remap from="scan" to="scan" /> -->
-  <!-- <remap from="scan_filtered" to="scan_filtered"/> -->
-  <!-- v1 -->
-  <!-- <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" /> -->
-  <!-- v2 -->
-  <!-- imaha iranai -->
   <include file="$(find cube_petit_bringup)/launch/realsense_rgbd.launch"/>
 
   <!-- launch CAMERA -->
@@ -49,4 +41,4 @@
     <arg name="config_file" value="cube_petit_analyzers.yaml"/>
   </include>
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/cube_petit_bringup/launch/rear_camera_bringup.launch b/cube_petit_bringup/launch/rear_camera_bringup.launch
index b014a80..fd852ce 100644
--- a/cube_petit_bringup/launch/rear_camera_bringup.launch
+++ b/cube_petit_bringup/launch/rear_camera_bringup.launch
@@ -1,15 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-
   <group ns="/rear_camera/rear_camera">
-    <!-- launch カメラ -->>
-    <!-- <node pkg="uvc_camera" type="uvc_camera_node" name="usb_camera_node">
-        <param name="device" type="string" value="/dev/rear_camera"/>
-        <param name="pixel_format" value="YUYV"/>
-        <param name="camera_frame_id" value="rear_camera"/>
-        <param name="io_method" value="mmap"/>
-        <param name="frame_id" value="rear_camera_optical_link"/>
-        <param name="camera_info_url" type="string" value="file://$(find cube_petit_bringup)/config/sensors/rear_camera/camera.yaml"/> </node>  -->
     <node pkg="usb_cam" type="usb_cam_node" name="usb_cam">
         <param name="device" type="string" value="/dev/rear_camera"/>
         <param name="pixel_format" value="yuyv"/>
@@ -19,21 +10,6 @@
         <param name="camera_info_url" type="string" value="file://$(find cube_petit_bringup)/config/sensors/rear_camera/camera.yaml"/> -->
     </node>
   </group>
-
-    <!-- ARマーカー Settings --><!-- marker size [cm]-->
-    <!-- <arg name="marker_size" default="5.5" />
-    <arg name="max_new_marker_error" default="0.08" />
-    <arg name="max_track_error" default="0.2" />
-    <arg name="cam_image_topic" default="/rear_camera/image_raw" />
-    <arg name="cam_info_topic" default="/rear_camera/camera_info" />
-    <arg name="output_frame" default="/rear_cameras" /> -->
-
-    <!-- launch AR_TRACK_ALVAR -->
-    <!-- <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen"
-    args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error)
-          $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" /> -->
-
-
 </launch>
 
 <!-- v4l2-ctl - - list-devices -->
diff --git a/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch b/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch
index d70f69b..43d8f89 100644
--- a/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch
+++ b/cube_petit_gazebo/launch/cube_petit_gazebo_pc.launch
@@ -1,14 +1,17 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <!-- load cube_petit urdf -->
-  <arg name="cube_petit_version1" default="true"/>
-  <arg name="cube_petit_version2" default="false"/>
+  <arg name="cube_petit_version" default="v2"/>
   <!-- version1 -->
-  <!-- <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit.xacro" /> -->
-  <!-- <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo.yaml"/> -->
+  <group if="$(arg cube_petit_version) == 'v1'">
+    <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit.xacro" />
+    <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo.yaml"/>
+  </group>
   <!-- version2 -->
-  <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit_v2.xacro"/>
-  <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo2.yaml"/>
+  <group if="$(arg cube_petit_version) == 'v2'">
+    <arg name="model" default="$(find cube_petit_description)/urdf/cube_petit_v2.xacro"/>
+    <rosparam command="load" file="$(find cube_petit_navigation)/config/odom_gazebo2.yaml"/>
+  </group>
   <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>
   <env name="GAZEBO_MODEL_PATH" value="$(find cube_petit_gazebo)/models:$(optenv GAZEBO_MODEL_PATH)"/>
   <!-- cut laser data of cube_petit's pole -->
@@ -16,9 +19,13 @@
     <remap from="scan" to="scan"/>
     <remap from="scan_filtered" to="scan_filtered"/>
     <!-- v1 -->
-    <!-- <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" /> -->
+    <group if="$(arg cube_petit_version) == 'v1'">
+      <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter.yaml" />
+    </group>
     <!-- v2 -->
-    <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter2.yaml"/>
+    <group if="$(arg cube_petit_version) == 'v2'">
+      <rosparam command="load" file="$(find cube_petit_bringup)/config/sensors/cube_petit_pacecat_filter2.yaml"/>
+    </group>
   </node>
   <arg name="model2" default="$(find cube_petit_gazebo)/models/charge_doc/cube_petit_charge_doc.xacro"/>
   <param name="robot_description2" command="$(find xacro)/xacro $(arg model2) --inorder"/>
@@ -47,15 +54,6 @@
     <node name="simple_goal_relay" pkg="topic_tools" type="relay" args="/move_base_simple/goal /move_base_simple/goal"/>
   </group>
   <!-- launch teleop -->
-  <!-- <group ns="joystick">
-      <arg name="teleop_config" default="$(find cube_petit_bringup)/config/ps3.config.yaml"/>
-      <node name="joy_node" pkg="joy" type="joy_node"/>
-      <node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
-        <rosparam command="load" file="$(arg teleop_config)" />
-        <remap from="cmd_vel" to="/cube_petit/diff_drive_controller/cmd_vel"/>
-      </node>
-    </group>
-     -->
   <group ns="joystick">
     <arg name="teleop_config" default="$(find cube_petit_bringup)/config/ps3.config.yaml"/>
     <param name="robot_description" command="$(find xacro)/xacro $(arg model) --inorder"/>
@@ -71,4 +69,4 @@
   <include file="$(find cube_petit_gazebo)/launch/include/docking_station_simulation.launch">
     <arg name="robot" value="cube-petit"/>
   </include>
-</launch>
\ No newline at end of file
+</launch>