Feature/tsukuba2025 #13

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open

kugurofu wants to merge 25 commits into main from feature/tsukuba2025

orange_bringup/config/motor_driver_params.yaml

-Original file line number
+Diff line change
@@ Expand Up / @@ -7,7 +7,7 @@ motor_driver_node: @@
         left_wheel_radius: 0.09767 # meter
         right_wheel_radius: 0.09767 # meter
         # For odometry computation
-        computation_left_wheel_radius: 0.0970 # meter
+        computation_left_wheel_radius: 0.09755 # meter
         computation_right_wheel_radius: 0.0973 # meter
         # Encoder CPR(counts per revolution)
         cpr: 16385
@@ Expand Down @@

orange_bringup/launch/imu_data.launch.xml

-Original file line number
+Diff line change
@@ -0,0 +1,52 @@
+    <?xml version="1.0"?>
+    <launch>
+      <!--# Control mode
+          # 1: relative position control mode
+          # Default: Subscribe to "/zlac8015d/pos/cmd_deg" and "/zlac8015d/pos/cmd_dist"
+          # 3: Speed rpm control mode
+          # Default: Subscribe to "/zlac8015d/twist/cmd_vel", "/zlac8015d/vel/cmd_vel" and "/zlac8015d/vel/cmd_rpm"-->
+      <arg name="control_mode" default="3"/>
+      <arg name="debug_motor" default="false"/>
+      <arg name="debug_imu" default="false"/>
+      <arg name="publish_TF" default="false"/>
+      <arg name="publish_odom" default="true"/>
+      <arg name="use_sim_time" default="false"/>
+      <arg name="twist_cmd_vel_topic" default="/cmd_vel"/>
+      <arg name="cmd_vel_topic" default="/vel/cmd_vel"/>
+      <arg name="cmd_rpm_topic" default="/vel/cmd_rpm"/>
+      <arg name="cmd_deg_topic" default="/pos/cmd_deg"/>
+      <arg name="cmd_dist_topic" default="/pos/cmd_dist"/>
+      <arg name="odom_topic" default="/odom"/>
+      <arg name="imu_topic" default="/imu"/>
+      <arg name="fusion_odom_topic" default="/localization/odom"/>
+      <arg name="ekf_publish_TF" default="false"/>
+      <arg name="hokuyo_scan_topic" default="/hokuyo_scan"/>
+      <arg name="velodyne_scan_topic" default="/velodyne_scan"/>
+      <arg name="velodyne_points_topic" default="/velodyne_points"/>
+      <arg name="livox_scan_topic" default="/livox_scan"/>
+      <arg name="livox_points_topic" default="/livox_points"/>
+      <arg name="merged_cloud_topic" default="/merged_cloud"/>
+      <arg name="merged_scan_topic" default="/merged_scan"/>
+      <arg name="nav_topic" default="/nav_state"/>
+      <arg name="estop_topic" default="/estop"/>
+      <arg name="xacro_file_path" default="$(find-pkg-share orange_description)/xacro/orange_robot.xacro"/>
+      <arg name="motor_driver_config_file_path" default="$(find-pkg-share orange_bringup)/config/motor_driver_params.yaml"/>
+      <!-- madgwick filter node -->
+      <node pkg="imu_filter_madgwick" exec="imu_filter_madgwick_node" name="madgwick_filter">
+        <param from="$(find-pkg-share cxd5602pwbimu)/config/madgwick.yaml"/>
+        <remap from="/imu/data_raw" to="/imu/spresense"/>
+        <remap from="/imu/data" to="/imu/spresense/madgwick"/>
+      </node>
+      <!-- initial_yaw_corrector -->
+      <node pkg="cxd5602pwbimu" exec="initial_yaw_corrector" name="spimu_yaw_corrector">
+        <param name="input_topic" value="/imu/spresense/madgwick"/>
+        <param name="output_topic" value="/imu/spresense/inityaw"/>
+      </node>
+      <!-- wheel_imu_odom for spresense imu -->
+      <node pkg="cxd5602pwbimu" exec="wheel_spimu_odom">
+        <param name="odom_topic" value="/odom"/>
+        <param name="imu_topic" value="/imu/spresense/inityaw"/>
+        <param name="publish_TF" value="false"/>
+      </node>
+    </launch>

orange_bringup/launch/orange_robot.launch.xml

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -58,13 +58,42 @@
  
        <param name="odom_child_frame" value="base_footprint"/>

      </node>

      <!-- estop  -->

      <node pkg="estop_ros" exec="cmd_vel_override_node" output="screen">

      <!--<node pkg="estop_ros" exec="cmd_vel_override_node" output="screen">

        <param name="port" value="/dev/sensors/estop"/>

        <param name="baudrate" value="115200"/>

        <param name="time_out" value="500"/>

        <param name="serial_interval" value="0.01"/>

        <remap from="/estop/state" to="/estop"/>

      </node>-->

      <!-- spresense imu -->

      <node pkg="cxd5602pwbimu" exec="cxd5602pwbimu_node">

        <param name="port" value="/dev/sensors/spresense_imu"/>

        <param name="frame_id" value="imu_link"/>

        <param name="imu_topic" value="/imu/spresense"/>

        <param name="time_out" value="10.0"/>

        <param name="baudrate" value="460800"/>

        <param name="gyro_bias_correction" value="true"/>

        <param name="gyro_bias_samples" value="4800"/>

      </node>

      <!-- madgwick filter node -->

      <node pkg="imu_filter_madgwick" exec="imu_filter_madgwick_node" name="madgwick_filter">

        <param from="$(find-pkg-share cxd5602pwbimu)/config/madgwick.yaml"/>

        <remap from="/imu/data_raw" to="/imu/spresense"/>

        <remap from="/imu/data" to="/imu/spresense/madgwick"/>

      </node>

      <!-- initial_yaw_corrector -->

      <node pkg="cxd5602pwbimu" exec="initial_yaw_corrector" name="spimu_yaw_corrector">

        <param name="input_topic" value="/imu/spresense/madgwick"/>

        <param name="output_topic" value="/imu/spresense/inityaw"/>

      </node>

      <!-- wheel_imu_odom for spresense imu -->

      <node pkg="cxd5602pwbimu" exec="wheel_spimu_odom">

        <param name="odom_topic" value="/odom"/>

        <param name="imu_topic" value="/imu/spresense/inityaw"/>

        <param name="publish_TF" value="false"/>

      </node>

      <!-- imu  -->

      <include file="$(find-pkg-share icm_20948)/launch/run.launch.xml">

        <arg name="port" value="/dev/sensors/imu"/>

    @@ -81,14 +110,18 @@
  
        <arg name="fusion_odom_out" value="$(var fusion_odom_topic)"/>

      </include>-->

      <!-- GPSposition_publisher -->

      <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 276.5 -->

      <!-- GPSheading_publisher -->

      <node pkg="orange_gnss" exec="gnss_odom_publisher_ttyUSB" output="screen">

      <!-- initial heading:housei nakaniwa=179.169287 tsukuba= 93.0 -->

      <!-- initial start_lat:housei nakaniwa=35.425952230280004 tsukuba right=36.04974095972727 tsukuba_left=36.04976195993636 -->

      <!-- initial start_lon:housei nakaniwa=139.31380123427 tsukuba right=140.04593633886364 tsukuba_left=140.04593755179093 -->

      <node pkg="orange_gnss" exec="gnss_odom_movingbase_fix_publisher_ttyUSB" output="screen">

        <param name="port" value="/dev/sensors/GNSS_UM982"/>

        <param name="baud" value="115200"/>

        <param name="country_id" value="0"/>

        <param name="Position_magnification" value="1.675"/>

        <param name="heading" value="0.0"/>

        <param name="heading" value="93.0"/>

        <param name="start_lat" value="36.04974095972727"/>

        <param name="start_lon" value="140.04593633886364"/>

        <param name="time_out" value="1.0"/>

      </node>

      <!-- 

    @@ -169,11 +202,11 @@
  
        <arg name="laserscan_topics" value="$(var hokuyo_scan_topic) $(var livox_scan_topic)"/>

      </include>-->

      <!-- led_node -->

      <node pkg="orange_sensor_tools" exec="led_node">

      <!--<node pkg="orange_sensor_tools" exec="led_node">

        <param name="use_sim_time" value="$(var use_sim_time)"/>

        <param name="serial_port" value="/dev/sensors/LED"/>

        <param name="baud_rate" value="115200"/>

        <param name="nav_topic" value="$(var nav_topic)"/>

        <param name="estop_topic" value="$(var estop_topic)"/>

      </node>

      </node>-->

    </launch>

orange_bringup/orange_bringup/motor_driver_node.py

-Original file line number
+Diff line change
@@ Expand Up / @@ -492,6 +492,7 @@ def get_wheels_travelled(self): @@
             )  # unit in meter
             return l_travelled, r_travelled
+        '''
         def modbus_fail_read_handler(self, ADDR, WORD):
             read_success = False
             reg = [None] * WORD
@@ Expand All / @@ -506,6 +507,34 @@ def modbus_fail_read_handler(self, ADDR, WORD): @@
                     self.get_logger().error(e)
                     pass
             return reg
+            '''
+        def modbus_fail_read_handler(self, ADDR, WORD, max_retry=5):
+            read_success = False
+            reg = [0] * WORD
+            retry_count = 0
+            while not read_success and retry_count < max_retry:
+                result = self.client.read_holding_registers(ADDR, WORD, slave=self.ID)
+                if result is None or not hasattr(result, "registers"):
+                    self.get_logger().warn(f"Modbus read failed (try {retry_count+1}/{max_retry})")
+                    time.sleep(0.2)
+                    retry_count += 1
+                    continue
+                try:
+                    for i in range(WORD):
+                        reg[i] = result.registers[i]
+                    read_success = True
+                except Exception as e:
+                    self.get_logger().error(f"Read error: {e}")
+                    retry_count += 1
+                    time.sleep(0.2)
+            if not read_success:
+                self.get_logger().error(f"Modbus read failed after {max_retry} retries. Stopping.")
+                raise RuntimeError("Modbus communication failed.")
+            return reg
         def set_rpm_with_limit(self, left_rpm, right_rpm):
             if self.left_rpm_lim < left_rpm:
@@ Expand Down @@

orange_description/xacro/orange_robot.xacro

-Original file line number
+Diff line change
@@ Expand Up / @@ -7,17 +7,24 @@ @@
       <xacro:sensor_hokuyo name="hokuyo" parent="base_link" min_angle="-1.22" max_angle="1.22" samples="720">
         <origin xyz="0.1075 0.0 0.1085" rpy="0.0 0.0 0.0"/>
       </xacro:sensor_hokuyo>
       <!--import 3D LiDAR model-->
       <!--bringup-->
       <xacro:include filename="$(find orange_description)/xacro/sensors/livox.xacro"/>
       <xacro:sensor_livox name="livox_frame" parent="base_link" min_angle="-2.35619" max_angle="2.35619" samples="720">
-        <origin xyz="-0.0185 0.0 0.8985" rpy="${pi} 0.0 0.0"/>
+        <origin xyz="-0.0185 0.0 0.94" rpy="${pi} 0.0 0.0"/>
+      </xacro:sensor_livox>
+      <xacro:sensor_livox name="livox_under" parent="base_link" min_angle="-2.35619" max_angle="2.35619" samples="720">
+        <origin xyz="-0.0185 0.0 0.67" rpy="${pi} 0.0 0.0"/>
       </xacro:sensor_livox>
       <!--simulation-->
       <!--<xacro:include filename="$(find ros2_livox_simulation)/urdf/mid360.xacro"/>
       <xacro:mid360 name="livox" parent="base_link" topic="mid360">
         <origin xyz="-0.0185 0.0 0.8985" rpy="${pi} 0.0 0.0"/>
       </xacro:mid360>-->
       <!--import imu model-->
       <xacro:include filename="$(find orange_description)/xacro/sensors/imu.xacro"/>
       <xacro:sensor_imu name="imu" parent="base_link" size="0.05 0.05 0.05">
@@ Expand Down @@

orange_gnss/orange_gnss/GPSodom_correction.py

-Original file line number
+Diff line change
@@ Expand Up / @@ -14,7 +14,7 @@ def __init__(self): @@
             super().__init__('GPSodom_correction')
             self.create_subscription(
-                Odometry, "/odom/gps", self.odomgps_callback, 1)
+                Odometry, "/odom/UM982", self.odomgps_callback, 1)
             self.create_subscription(
                 Imu, "/livox/imu", self.imu_callback, 1)
@@ Expand Down @@

orange_gnss/orange_gnss/gnss_odom_movingbase_fix_publisher_ttyUSB.py

            
                      Original file line number
                      Diff line number
                      Diff line change
                  
    @@ -16,24 +16,29 @@ class GPSData(Node):
  
        def __init__(self):

            super().__init__('gps_data_acquisition')

            # Parameters

            self.declare_parameter('port', '/dev/sensors/GNSS_UM982')

            self.declare_parameter('baud', 115200)

            self.declare_parameter('country_id', 0)

            self.declare_parameter('Position_magnification', 1.675)

            self.declare_parameter('heading', 90.0)

            self.declare_parameter('start_lat', 35.425952230280004) # tsukuba start point right 36.04974095972727, 140.04593633886364 , left 36.04976195993636, 140.04593755179093/nakaniwa 35.4257898377487,139.313807281254 /35.425952230280004, 139.31380123427

            self.declare_parameter('start_lon', 139.31380123427)

            self.dev_name = self.get_parameter('port').get_parameter_value().string_value

            self.serial_baud = self.get_parameter('baud').get_parameter_value().integer_value

            self.country_id = self.get_parameter('country_id').get_parameter_value().integer_value

            self.Position_magnification = self.get_parameter('Position_magnification').get_parameter_value().double_value

            self.theta = self.get_parameter('heading').get_parameter_value().double_value

            #self.theta = 275.6 # tukuba param

            #self.theta = 180 #nakaniwa param

            #self.theta = self.get_parameter('heading').get_parameter_value().double_value

            self.tsukuba_theta= self.get_parameter('heading').get_parameter_value().double_value # nakaniwa 180 tsukuba 93

            self.theta = self.tsukuba_theta

            self.initial_coordinate = None

            self.start_lat = self.get_parameter('start_lat').get_parameter_value().double_value

            self.start_lon = self.get_parameter('start_lon').get_parameter_value().double_value

            self.start_GPS_coordinate = [self.start_lat, self.start_lon]

            self.fix_data = None

            self.count = 0

            self.initialized = False  # 平均初期座標が取得できたかどうか

            # Publishers

    @@ -46,9 +51,12 @@ def __init__(self):
  
            # service client

            self.client = self.create_client(Avglatlon, 'send_avg_gps')

            while not self.client.wait_for_service(timeout_sec=1.0):

                self.get_logger().info("service not available...")

            #while not self.client.wait_for_service(timeout_sec=1.0):

            #    self.get_logger().info("service not available...")

            self.get_logger().info("Start get_lonlat quat node")

            self.get_logger().info("-------------------------")

            # Timers

            self.timer = self.create_timer(1.0, self.timer_callback)

    @@ -62,7 +70,7 @@ def __init__(self):
  
            # tkinter GUI setup

            self.root = tk.Tk()

            self.root.title("GPS Data Acquisition")

            self.start_button = tk.Button(self.root, text="Start", command=self.start_gps_acquisition, width=20, height = 5)

            self.start_button = tk.Button(self.root, text="Start GPS Acquisition", command=self.start_gps_acquisition, width=20, height = 5)

            self.start_button.pack()

            self.gps_acquisition_thread = None

    @@ -73,7 +81,11 @@ def send_request(self):
  
            request = Avglatlon.Request()

            request.avg_lat = self.initial_coordinate[0]  # ← average lat

            request.avg_lon = self.initial_coordinate[1]  # ← average lon

            request.theta = self.theta

            request.current_lat = self.current_coordinate[0]  # ← currennt lat

            request.current_lon = self.current_coordinate[1]  # ← currennt lon

            #request.theta = self.theta

            request.theta = self.tsukuba_theta # tsukuba start theta

            request.current_theta = self.theta # for tsukuba

            future = self.client.call_async(request)

            future.add_done_callback(self.response_callback)

    @@ -96,7 +108,7 @@ def timer_callback(self):
  
            self.gps_data_cache = self.get_gps_quat(self.dev_name, self.country_id)

            if self.gps_data_cache:

                fix_type, lat, lon, alt, _, heading = self.gps_data_cache

                fix_type, lat, lon, alt, satelitecount_data, heading = self.gps_data_cache

                if fix_type != 0:

                    self.publish_fix(self.gps_data_cache)

                    self.publish_odom(lat, lon, alt)

    @@ -124,13 +136,19 @@ def acquire_gps_data(self):
  
                if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0:

                    lat_sum += GPS_data[1]

                    lon_sum += GPS_data[2]

                    #heading_sum += float(GPS_data[5])

                    count += 1

                time.sleep(0.1)  # Slight delay to avoid overwhelming the GPS device

            if count > 0:

                self.initial_coordinate = [lat_sum / count, lon_sum / count]

                #self.initial_coordinate = [lat_sum / count, lon_sum / count] # calculate average

                self.initial_coordinate = self.start_GPS_coordinate

                self.current_coordinate = [lat_sum / count, lon_sum / count] # for tsukuba

                #self.theta = (heading_sum / count) - 90

                self.initialized = True

                self.get_logger().info(f"Initial coordinate set to: {self.initial_coordinate}")

                self.get_logger().info(f"current coordinate set to: {self.current_coordinate}")

                self.get_logger().info(f"Initial theta set to: {self.tsukuba_theta}")

                self.send_request()

            self.is_acquiring = False

    @@ -250,7 +268,7 @@ def quaternion_from_euler(self, roll, pitch, yaw):
  
        def heading_to_quat(self ,real_heading):

            robotheading = real_heading + 90

            robotheading = real_heading - 90

            if robotheading >= 360:

                robotheading -= 360

    @@ -259,7 +277,8 @@ def heading_to_quat(self ,real_heading):
  
            if self.count == 0:

                self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!")

                self.first_heading = robotheading

                #self.first_heading = robotheading

                self.first_heading = self.tsukuba_theta

                self.count = 1

            relative_heading = robotheading - self.first_heading

    @@ -358,6 +377,8 @@ def publish_fix(self, gps):
  
            self.lonlat_msg.latitude = gps[1]

            self.lonlat_msg.longitude = gps[2]

            self.lonlat_msg.altitude = gps[3]

            self.lonlat_msg.status.service = gps[4] # satelitecount

            self.lonlat_pub.publish(self.lonlat_msg)

                # self.get_logger().info(f"Published GPS data: {lonlat}")

    @@ -366,13 +387,13 @@ def publish_fix(self, gps):
  
        def publish_movingbase(self, heading):

            if heading is not None and heading != 0.0:

                robotheading = heading + 90.0

                robotheading = heading - 90.0

                if robotheading >= 360.0:

                    robotheading -= 360.0

                if self.count == 0:

                    self.get_logger().info(f"!!!----------robotheading: {robotheading} deg----------!!!")

                    self.first_heading = robotheading

                    self.first_heading = self.tsukuba_theta

                    self.count = 1

                relative_heading = robotheading - self.first_heading

    @@ -409,8 +430,7 @@ def publish_movingbase(self, heading):
  
                self.get_logger().error("!!!!-not movingbase data-!!!!")

        def publish_odom(self, lat, lon, alt):

        def publish_odom(self, lat, lon, alt):        

            #GPS_data = self.get_gps_quat(self.dev_name, self.country_id)

            #gnggadata = (Fixtype_data,latitude_data,longitude_data,altitude_data,satelitecount_data,heading)

            #if GPS_data and GPS_data[1] != 0 and GPS_data[2] != 0:

    @@ -419,9 +439,9 @@ def publish_odom(self, lat, lon, alt):
  
                self.satelite = GPS_data[4]

                lonlat = [GPS_data[1], GPS_data[2]]

                if self.initial_coordinate is None:

                    self.initial_coordinate = [GPS_data[1], GPS_data[2]]        

                GPSxy = self.conversion(lonlat, self.initial_coordinate, self.theta)

                #if self.initial_coordinate is None:

                #    self.initial_coordinate = [GPS_data[1], GPS_data[2]]        

                GPSxy = self.conversion(lonlat, self.start_GPS_coordinate, self.tsukuba_theta)

                GPSquat = self.heading_to_quat(GPS_data[5])       

                self.odom_msg.header.stamp = self.get_clock().now().to_msg()

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/tsukuba2025 #13

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!

Feature/tsukuba2025 #13

Are you sure you want to change the base?

Uh oh!

Feature/tsukuba2025 #13

Uh oh!

Uh oh!

Diff view

Diff view

There are no files selected for viewing

Uh oh!