ethz-asl · Jaeyoung-Lim · Mar 25, 2024
diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h b/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h
@@ -66,6 +66,7 @@
 #include "snowsampler_msgs/TakeMeasurement.h"
 #include "snowsampler_msgs/Trigger.h"
 
+#include <sensor_msgs/Range.h>
 // #include "px4_msgs/msg/distance_sensor.hpp"
 // #include "px4_msgs/msg/vehicle_attitude.hpp"
 // #include "px4_msgs/msg/vehicle_command.hpp"
@@ -120,7 +121,7 @@ class AdaptiveSnowSampler {
 
   void vehicleGlobalPositionCallback(const sensor_msgs::NavSatFix &msg);
 
-  // void distanceSensorCallback(const px4_msgs::DistanceSensor &msg);
+  void distanceSensorCallback(const sensor_msgs::Range &msg);
 
   bool startPositionCallback(planner_msgs::SetVector3::Request &request, planner_msgs::SetVector3::Response &response);
 
@@ -204,6 +205,7 @@ class AdaptiveSnowSampler {
   ros::Publisher snow_depth_pub_;
   ros::Publisher vehicle_pose_pub_;
   ros::Publisher map_info_pub_;
+  ros::Publisher distance_measurement_pub_;
 
   ros::Subscriber vehicle_attitude_sub_;
   ros::Subscriber vehicle_global_position_sub_;
@@ -246,6 +248,8 @@ class AdaptiveSnowSampler {
   Eigen::Vector3d home_position_{Eigen::Vector3d(0.0, 0.0, 0.0)};
   Eigen::Vector3d home_normal_{Eigen::Vector3d(0.0, 0.0, 0.0)};
 
+  std::vector<Eigen::Vector3d> measured_points_;
+
   // tilt prevention parameters
   double tilt_treshold_{0.035};  // ~2deg
   double tilt_window_size_{3};

diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/visualization.h b/adaptive_snowsampler/include/adaptive_snowsampler/visualization.h
@@ -92,4 +92,27 @@ inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d&
   pub.publish(marker_array);
 }
 
+inline void publishDistanceMeasurements(const ros::Publisher pub, const std::vector<Eigen::Vector3d>& points) {
+  visualization_msgs::Marker marker;
+    marker.header.stamp = ros::Time::now();
+    marker.header.frame_id = "map";
+    marker.type = visualization_msgs::Marker::SPHERE_LIST;
+    marker.ns = "distance_sensor";
+    marker.scale.x = 0.5;
+    marker.scale.y = 0.5;
+    marker.scale.z = 0.5;
+    marker.color.a = 1.0;  // Don't forget to set the alpha!
+    marker.color.r = 1.0;
+    marker.color.g = 0.0;
+    marker.color.b = 0.0;
+  for (const auto &point : points) {
+    geometry_msgs::Point point_msg;
+    point_msg.x = point.x();
+    point_msg.y = point.y();
+    point_msg.z = point.z();
+    marker.points.push_back(point_msg);
+  }
+  pub.publish(marker);
+}
+
 #endif
diff --git a/adaptive_snowsampler/scripts/log_replay.py b/adaptive_snowsampler/scripts/log_replay.py
@@ -11,13 +11,21 @@
 import rosbag # pylint: disable=import-error
 # from px4_msgs import msg as px4_msgs # pylint: disable=import-error
 from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import Range
 from geometry_msgs.msg import PoseStamped
 import os
+import numpy as np
+import math
 
 from pyulog import core
 
 #pylint: disable=too-many-locals, invalid-name
 
+def quatMultiplication(q, p):
+  quat = np.array([p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3], p[0] * q[1] + p[1] * q[0] - p[2] * q[3] + p[3] * q[2],
+      p[0] * q[2] + p[1] * q[3] + p[2] * q[0] - p[3] * q[1], p[0] * q[3] - p[1] * q[2] + p[2] * q[1] + p[3] * q[0]])
+  return quat
+
 def main():
     """Command line interface"""
 
@@ -43,7 +51,7 @@ def parseData(d, topic, idx):
 
 
 def appendBag(path, bag):
-        msg_filter={'vehicle_global_position', 'vehicle_attitude'}
+        msg_filter={'vehicle_global_position', 'vehicle_attitude', 'distance_sensor'}
         disable_str_exceptions=False
         ulog = core.ULog(path, msg_filter, disable_str_exceptions)
         data = ulog.data_list
@@ -65,11 +73,18 @@ def appendBag(path, bag):
                 elif d.name == "vehicle_attitude":
                     topic = "mavros/local_position/pose"
                     msg = PoseStamped()
-                    # msg.pose.orientation.w
-                    msg.pose.orientation.w = parseData(d, 'q[0]', i)
-                    msg.pose.orientation.x = parseData(d, 'q[1]', i)
-                    msg.pose.orientation.y = parseData(d, 'q[2]', i)
-                    msg.pose.orientation.z = parseData(d, 'q[3]', i)
+                    vehicle_attitude = np.array([parseData(d, 'q[0]', i), parseData(d, 'q[1]', i), -parseData(d, 'q[2]', i), -parseData(d, 'q[3]', i)])
+                    attitude_offset = np.array([np.cos(0.5 * 0.5 * math.pi), 0.0, 0.0, np.sin(0.5 * 0.5 * math.pi)])
+                    vehicle_aligned_attitude = quatMultiplication(attitude_offset, vehicle_attitude)
+                    msg.pose.orientation.w = vehicle_aligned_attitude[0]
+                    msg.pose.orientation.x = vehicle_aligned_attitude[1]
+                    msg.pose.orientation.y = vehicle_aligned_attitude[2]
+                    msg.pose.orientation.z = vehicle_aligned_attitude[3]
+
+                elif d.name == "distance_sensor":
+                    topic = "mavros/distance_sensor"
+                    msg = Range()
+                    msg.range = parseData(d, 'current_distance', i)
 
                 # for f in d.field_data:
                 #     result = array_pattern.match(f.field_name)

diff --git a/adaptive_snowsampler/src/adaptive_snowsampler.cpp b/adaptive_snowsampler/src/adaptive_snowsampler.cpp
@@ -69,16 +69,16 @@ AdaptiveSnowSampler::AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::N
   referencehistory_pub_ = nh_.advertise<nav_msgs::Path>("reference/path", 1);
   snow_depth_pub_ = nh_.advertise<std_msgs::Float64>("/snow_depth", 1);
   vehicle_pose_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("vehicle_pose_marker", 1);
+  distance_measurement_pub_ = nh_.advertise<visualization_msgs::Marker>("distance_measurements", 1);
 
   // Subscribers
   vehicle_global_position_sub_ =
       nh_.subscribe("mavros/global_position/global", 1, &AdaptiveSnowSampler::vehicleGlobalPositionCallback, this,
                     ros::TransportHints().tcpNoDelay());
   vehicle_attitude_sub_ = nh_.subscribe("mavros/local_position/pose", 1, &AdaptiveSnowSampler::vehicleAttitudeCallback,
                                         this, ros::TransportHints().tcpNoDelay());
-  // distance_sensor_sub_ = this->create_subscription<px4_msgs::DistanceSensor>(
-  //     "/fmu/out/distance_sensor", qos_profile,
-  //     std::bind(&AdaptiveSnowSampler::distanceSensorCallback, this, std::placeholders::_1));
+  distance_sensor_sub_ = nh_.subscribe("mavros/distance_sensor", 1, &AdaptiveSnowSampler::distanceSensorCallback, this,
+                                    ros::TransportHints().tcpNoDelay());
   mavcmd_long_service_client_ = nh_.serviceClient<mavros_msgs::CommandLong>("mavros/cmd/command");
   mavcmd_int_service_client_ = nh_.serviceClient<mavros_msgs::CommandInt>("mavros/cmd/command_int");
 
@@ -143,6 +143,7 @@ void AdaptiveSnowSampler::cmdloopCallback(const ros::TimerEvent &event) {
     publishPositionHistory(referencehistory_pub_, vehicle_position_, positionhistory_vector_);
   }
   publishVehiclePose(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_);
+  publishDistanceMeasurements(distance_measurement_pub_, measured_points_);
 }
 
 void AdaptiveSnowSampler::statusloopCallback(const ros::TimerEvent &event) {
@@ -341,9 +342,23 @@ void AdaptiveSnowSampler::vehicleGlobalPositionCallback(const sensor_msgs::NavSa
   // Send the transformation
   // tf_broadcaster_->sendTransform(t);
 }
-// void AdaptiveSnowSampler::distanceSensorCallback(const px4_msgs::DistanceSensor &msg) {
-//   lidar_distance_ = msg.current_distance;
-// }
+
+void AdaptiveSnowSampler::distanceSensorCallback(const sensor_msgs::Range &msg) {
+  lidar_distance_ = msg.range;
+  ///TODO: Project lidar measurement to the ground
+  Eigen::Vector3d distance_vector(0.0, 0.0, -msg.range);
+  const auto R = vehicle_attitude_.toRotationMatrix();
+  Eigen::Vector3d projected_distance_vector = R.transpose() * distance_vector + vehicle_position_;
+  std::cout << "lidar distance: " << lidar_distance_ << std::endl;
+  std::cout << "  - projected_distance_vector: " << projected_distance_vector.transpose() << std::endl;
+  std::cout << "  - vehicle_position_: " << vehicle_position_.transpose() << std::endl;
+  if (map_initialized_ && map_->getGridMap().isInside(vehicle_position_.head(2))) {    
+    double dem_elevation = map_->getGridMap().atPosition("elevation", vehicle_position_.head(2));
+    std::cout << "  - Snow depth: " << projected_distance_vector.z() - dem_elevation << std::endl;
+  }
+
+  measured_points_.push_back(projected_distance_vector);
+}
 
 bool AdaptiveSnowSampler::takeMeasurementCallback(snowsampler_msgs::Trigger::Request &request,
                                                   snowsampler_msgs::Trigger::Response &response) {

diff --git a/snowsampler_rviz/launch/config.rviz b/snowsampler_rviz/launch/config.rviz
@@ -12,8 +12,9 @@ Panels:
         - /Marker1
         - /Marker2
         - /MarkerArray1
+        - /Marker4
       Splitter Ratio: 0.5
-    Tree Height: 161
+    Tree Height: 289
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -125,31 +126,40 @@ Visualization Manager:
       Marker Topic: /home_position
       Name: Marker
       Namespaces:
-        {}
+        sphere: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: /setpoint_position
       Name: Marker
       Namespaces:
-        {}
+        sphere: true
       Queue Size: 100
       Value: true
     - Class: rviz/Marker
       Enabled: true
       Marker Topic: /target_normal
       Name: Marker
       Namespaces:
-        {}
+        arrow: true
       Queue Size: 100
       Value: true
     - Class: rviz/MarkerArray
       Enabled: true
       Marker Topic: /vehicle_pose_marker
       Name: MarkerArray
       Namespaces:
-        {}
+        body: true
+        leg: true
+      Queue Size: 100
+      Value: true
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /distance_measurements
+      Name: Marker
+      Namespaces:
+        distance_sensor: true
       Queue Size: 100
       Value: true
   Enabled: true
@@ -180,35 +190,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 242.64480590820312
+      Distance: 360.1125793457031
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 15.703259468078613
-        Y: 103.41791534423828
-        Z: 1887.617431640625
+        X: -52.85272979736328
+        Y: -123.13216400146484
+        Z: 2161.042724609375
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5903987884521484
+      Pitch: 0.380399227142334
       Target Frame: <Fixed Frame>
-      Yaw: 5.393568515777588
+      Yaw: 1.3103853464126587
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 842
+  Height: 1136
   Hide Left Dock: false
   Hide Right Dock: false
   PlanningPanel:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001c4000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000012a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0050006c0061006e006e0069006e006700500061006e0065006c010000016b000001800000018000ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056a0000003efc0100000002fb0000000800540069006d006501000000000000056a0000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000028b000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001c4000003d6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0050006c0061006e006e0069006e006700500061006e0065006c01000001eb000002260000018000ffffff000000010000010f000003d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000459000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -217,6 +227,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1386
-  X: 432
-  Y: 82
+  Width: 1848
+  X: 72
+  Y: 27