Merge branch 'practice-mission-04-06' into integration

ansible/roles/jetson_services/files/rules/99-usb-serial.rules

@@ -0,0 +1,3 @@
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1546", ATTRS{idProduct}=="01a9", SYMLINK+="gps_%n"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0058", SYMLINK+="imu"
+SUBSYSTEM=="video4linux", KERNEL=="video*", ATTRS{idVendor}=="1bcf", ATTRS{idProduct}=="0b12", ATTR{index}=="0", GROUP="video", SYMLINK+="arducam" 

ansible/roles/jetson_services/tasks/main.yml

@@ -0,0 +1,14 @@
+- name: USB Rules
+  become: true
+  synchronize:
+    src: files/rules/
+    dest: /etc/udev/rules.d
+    recursive: true
+
+- name: udev Reload Rules
+  become: true
+  command: udevadm control --reload-rules
+
+- name: udev TTY Trigger
+  become: true
+  command: udevadm trigger --subsystem-match=tty

config/esw.yaml

@@ -1,14 +1,23 @@
+right_gps_driver:
+  port: "/dev/ttyACM0"
+  baud: 38400
+  frame_id: "base_link"
+
+left_gps_driver:
+  port: "/dev/gps_0"
+  baud: 38400
+  frame_id: "base_link"
+
+basestation_gps_driver:
+  port: "/dev/ttyUSB1"
+  baud: 38400
+  frame_id: "base_link"
+
 imu_driver:
   port: "/dev/imu"
   baud: 115200
   frame_id: "imu_link"
 
-gps:
-  port: "/dev/gps"
-  baud: 115200
-  useRMC: false # get covariance instead of velocity, see wiki for more info
-  frame_id: "base_link"
-
 can:
   devices:
     - name: "jetson"

config/localization.yaml

@@ -8,3 +8,4 @@ gps_linearization:
   reference_point_longitude: -83.7096706
   reference_point_altitude: 234.1
   reference_heading: 90.0
+  use_both_gps: false

launch/esw_base.launch

@@ -1,12 +1,6 @@
 <launch>
   <rosparam command="load" file="$(find mrover)/config/esw.yaml"/>
 
-  <!-- Bridges between ROS and SocketCAN -->
-  <!-- <node name="can_driver_0" pkg="mrover" type="can_driver"
-        respawn="true" respawn_delay="2"
-        output="screen">
-        <param name="interface" value="can0" />
-  </node> -->
   <node name="can_driver_1" pkg="mrover" type="can_driver"
         respawn="true" respawn_delay="2"
         output="screen">

launch/localization.launch

@@ -2,6 +2,7 @@
     <arg name="sim" default="false" />
     <arg name="use_ekf" default="true" />
     <arg name="ekf_start_delay" default="0" />
+    <arg name="use_both_gps" default="false" />
 
     <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
     <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
@@ -25,9 +26,13 @@
     <!-- Launch IMU driver -->
     <node unless="$(arg sim)" name="imu_driver" pkg="mrover" type="imu_driver.py" output="screen" />
 
-    <!-- Launch GPS driver -->
-    <group ns="gps">
-        <node unless="$(arg sim)" name="gps_driver" pkg="mrover" type="gps_driver.py" output="screen" />
+    <!-- Launch GPS driver(s) -->
+    <group ns="right_gps_driver" unless="$(arg sim)">
+        <node if="$(arg use_both_gps)" pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+    </group>
+
+    <group ns="left_gps_driver" unless="$(arg sim)">
+        <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
     </group>
 
 </launch>

launch/rtk_basestation.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+
+    <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
+    <node pkg="mrover" type="basestation_gps_driver.py" name="basestation_gps_driver"
+        output="screen" />
+
+</launch>

launch/rtk_rover.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<launch>
+  <arg name="use_both_gps" default="true" />
+
+  <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
+  <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
+  <rosparam command="load" file="$(find mrover)/config/ekf.yaml" />
+
+  <group ns="right_gps_driver">
+    <node if="$arg use_both_gps" pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+  </group>
+
+  <group ns="left_gps_driver">
+    <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+  </group>
+
+  <node pkg="mrover" type="gps_linearization.py" name="gps_linearization" output="screen"/>
+
+</launch>

msg/rtkStatus.msg

@@ -0,0 +1,5 @@
+uint8 NO_RTK = 0
+uint8 FLOATING_RTK = 1
+uint8 RTK_FIX = 2
+
+uint8 RTK_FIX_TYPE

package.xml

@@ -64,12 +64,16 @@
   <!-- Localization -->
   <depend>rviz_imu_plugin</depend>
   <depend>robot_localization</depend>
+  <depend>rtcm_msgs</depend>
 
   <!-- Navigation -->
 
   <!-- Utility -->
   <exec_depend>teleop_twist_joy</exec_depend>
   <exec_depend>teleop_twist_keyboard</exec_depend>
+  <depend>rqt_tf_tree</depend>
+  <depend>rosbag</depend>
+  <depend>rqt_bag</depend>
 
   <!-- Test -->
   <test_depend>rosunit</test_depend>

pyproject.toml

@@ -26,6 +26,7 @@ dependencies = [
     "aenum==3.1.15",
     "daphne==4.0.0",
     "channels==4.0.0",
+    "pyubx2==1.2.35",
     "opencv-python==4.9.0.80",
 ]
 

scripts/bag_to_csv.py

@@ -0,0 +1,95 @@
+import rosbag
+import csv
+
+# bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag"
+
+# bag = rosbag.Bag(bag_file_path)
+
+# topics = ["/left_gps_driver/fix", "/right_gps_driver/fix"]
+
+# csv_file_path = "stationary.csv"
+
+# with open(csv_file_path, "w", newline="") as csv_file:
+#     # Create a CSV writer
+#     csv_writer = csv.writer(csv_file)
+
+#     csv_writer.writerow(["GPS Type", "Timestamp", "Latitude", "Longitude", "Altitude"])
+
+#     for i in topics:
+#         for _, msg, timestamp in bag.read_messages(topics=i):
+#             # Extract relevant data from the message
+#             timestamp_secs = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9
+#             latitude = msg.latitude
+#             longitude = msg.longitude
+#             altitude = msg.altitude
+#             csv_writer.writerow([i, timestamp_secs, latitude, longitude, altitude])
+
+# bag.close()
+
+
+# bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag"
+
+# bag = rosbag.Bag(bag_file_path)
+
+# topic = "/linearized_pose"
+
+# csv_file_path = "pose.csv"
+
+# with open(csv_file_path, "w", newline="") as csv_file:
+#     csv_writer = csv.writer(csv_file)
+
+#     csv_writer.writerow(
+#         [
+#             "Timestamp",
+#             "Position_X",
+#             "Position_Y",
+#             "Position_Z",
+#             "Orientation_X",
+#             "Orientation_Y",
+#             "Orientation_Z",
+#             "Orientation_W",
+#         ]
+#     )
+
+#     for _, msg, timestamp in bag.read_messages(topics=topic):
+#         timestamp_secs = msg.header.stamp.secs + msg.header.stamp.nsecs * 1e-9
+#         pos_x = msg.pose.pose.position.x
+#         pos_y = msg.pose.pose.position.y
+#         pos_z = msg.pose.pose.position.z
+#         ori_x = msg.pose.pose.orientation.x
+#         ori_y = msg.pose.pose.orientation.y
+#         ori_z = msg.pose.pose.orientation.z
+#         ori_w = msg.pose.pose.orientation.w
+
+#         csv_writer.writerow([timestamp_secs, pos_x, pos_y, pos_z, ori_x, ori_y, ori_z, ori_w])
+
+# bag.close()
+
+bag_file_path = "../../short_range_rtk_2024-01-28-14-32-22.bag"
+
+bag = rosbag.Bag(bag_file_path)
+
+topics = ["/left_gps_driver/rtk_fix_status", "/right_gps_driver/rtk_fix_status"]
+
+csv_file_left = "left_fix_status.csv"
+csv_file_right = "right_fix_status.csv"
+
+with open(csv_file_left, "w", newline="") as csv_file:
+    csv_writer = csv.writer(csv_file)
+
+    csv_writer.writerow(["Topic Name", "RTK_fix"])
+
+    for _, msg, _ in bag.read_messages(topics="/left_gps_driver/rtk_fix_status"):
+        rtk_fix_type = int(msg.RTK_FIX_TYPE)
+        csv_writer.writerow(["/left_gps_driver/rtk_fix_status", rtk_fix_type])
+
+with open(csv_file_right, "w", newline="") as csv_file:
+    csv_writer = csv.writer(csv_file)
+
+    csv_writer.writerow(["Topic Name", "RTK_fix"])
+
+    for _, msg, _ in bag.read_messages(topics="/right_gps_driver/rtk_fix_status"):
+        rtk_fix_type = int(msg.RTK_FIX_TYPE)
+        csv_writer.writerow(["/right_gps_driver/rtk_fix_status", rtk_fix_type])
+
+bag.close()

scripts/gps_plotting.py

@@ -0,0 +1,112 @@
+# import matplotlib.pyplot as plt
+# from pymap3d.enu import geodetic2enu
+# import pandas as pd
+# import numpy as np
+
+# ref_lat = 42.293195
+# ref_lon = -83.7096706
+# ref_alt = 234.1
+
+# linearized_pose = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/linearized_pose.csv")
+
+
+# lin_pose_x = linearized_pose["Orientation_X"].to_numpy()
+# lin_pose_y = linearized_pose["Orientation_Y"].to_numpy()
+# lin_pose_z = linearized_pose["Orientation_Z"].to_numpy()
+# lin_pose_w = linearized_pose["Orientation_W"].to_numpy()
+
+# time = linearized_pose["Timestamp"].to_numpy()
+
+
+# rec_yaw = []
+# rec_time = []
+
+# for x, y, z, w, time in zip(lin_pose_x, lin_pose_y, lin_pose_z, lin_pose_w, time):
+#     siny_cosp = 2 * (w * z + x * y)
+#     cosy_cosp = 1 - 2 * (y * y + z * z)
+#     yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+#     rec_yaw.append(yaw)
+#     rec_time.append(time)
+
+# for i in range(1, len(rec_time)):
+#     rec_time[i] = rec_time[i] - rec_time[0]
+
+# rec_time[0] = 0
+
+# plt.figure(figsize=(10, 10))
+# plt.plot(rec_time, rec_yaw, color="red", label="right")
+# plt.xlabel("time (s)")
+# plt.ylabel("yaw (rad)")
+# plt.title("yaw vs time stationary")
+# plt.legend()
+# plt.savefig("rtk_plot.png")
+# plt.show()
+
+import matplotlib.pyplot as plt
+from pymap3d.enu import geodetic2enu
+import pandas as pd
+import numpy as np
+
+ref_lat = 42.293195
+ref_lon = -83.7096706
+ref_alt = 234.1
+gps_readings = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/stationary.csv")
+left_fix = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/left_fix_status.csv")
+right_fix = pd.read_csv("/home/daniel/catkin_ws/src/mrover/src/localization/right_fix_status.csv")
+
+left = left_fix["RTK_fix"].to_numpy()
+right = right_fix["RTK_fix"].to_numpy()
+
+left_arr = []
+right_arr = []
+for i in left_fix:
+    left_arr.append(i)
+for j in right_fix:
+    right_arr.append(i)
+
+gps_time = gps_readings["Timestamp"].to_numpy()
+gps_type = gps_readings["GPS Type"].to_numpy()
+gps_lat = gps_readings["Latitude"].to_numpy()
+gps_long = gps_readings["Longitude"].to_numpy()
+gps_alt = gps_readings["Altitude"].to_numpy()
+print(len(left), len(right), len(gps_time))
+
+left_time = []
+right_time = []
+left_x = []
+left_y = []
+left_z = []
+right_x = []
+right_y = []
+right_z = []
+
+for gps_type, lat, lon, alt, time in zip(gps_type, gps_lat, gps_long, gps_alt, gps_time):
+    if gps_type == "/left_gps_driver/fix":
+        pose = geodetic2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt, deg=True)
+        left_x.append(pose[0])
+        left_y.append(pose[1])
+        left_z.append(pose[2])
+        left_time.append(time)
+    else:
+        pose = geodetic2enu(lat, lon, alt, ref_lat, ref_lon, ref_alt, deg=True)
+        right_x.append(pose[0])
+        right_y.append(pose[1])
+        right_z.append(pose[2])
+        right_time.append(time)
+
+
+plt.figure(figsize=(10, 10))
+plt.scatter(left_time, left_x, left_y, color="black", label="left x and left y")
+# plt.scatter(left_time, left_y, color="orange", label="left y")
+plt.scatter(right_time, right_x, right_y, color="green", label="right x and right y")
+# plt.scatter(right_time, right_y, color="blue", label="right x")
+plt.scatter(left_time, left, color="red", label="left fix")
+plt.scatter(right_time, right, color="violet", label="right fix")
+# plt.scatter(left_x, left_y, color='blue', label='left', s=1)
+plt.xlabel("time")
+plt.ylabel("pos")
+plt.title("RTK Stationary test")
+plt.legend()
+plt.show()
+# plt.savefig("rtk_plot.png")