Feature/update bringup

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

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

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
+# 16:ps

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
+    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

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

cube_petit_bringup/launch/cube_petit_bringup.launch

@@ -8,24 +8,22 @@
   <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"/> -->
 
+  <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 +34,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>
+</launch>

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>

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>

cube_petit_bringup/launch/rear_camera_bringup.launch

@@ -1,35 +1,15 @@
 <?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/video0"/>
-        <param name="pixel_format" value="YUYV"/>
+    <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="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 -->
@@ -64,7 +44,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

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>

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>
-