From dcda447a19bb19835697ea87382ff4b7e0c4b487 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Mon, 25 Mar 2024 17:47:02 +0100 Subject: [PATCH] Do not append position if global position not received Run rviz Visualize distance sensor messages Visualize laser range points Display laser range finder points Fix quaternion alignment --- .../adaptive_snowsampler.h | 6 ++- .../adaptive_snowsampler/visualization.h | 23 ++++++++++ adaptive_snowsampler/scripts/log_replay.py | 27 +++++++++--- .../src/adaptive_snowsampler.cpp | 27 +++++++++--- snowsampler_rviz/launch/config.rviz | 42 ++++++++++++------- 5 files changed, 96 insertions(+), 29 deletions(-) diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h b/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h index 83bdac2..0c58736 100644 --- 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 // #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 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 index e7fb16f..88f630a 100644 --- 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& 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 index 07684fd..477e9e9 100755 --- 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 index fad3fde..58f031d 100644 --- a/adaptive_snowsampler/src/adaptive_snowsampler.cpp +++ b/adaptive_snowsampler/src/adaptive_snowsampler.cpp @@ -69,6 +69,7 @@ AdaptiveSnowSampler::AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::N referencehistory_pub_ = nh_.advertise("reference/path", 1); snow_depth_pub_ = nh_.advertise("/snow_depth", 1); vehicle_pose_pub_ = nh_.advertise("vehicle_pose_marker", 1); + distance_measurement_pub_ = nh_.advertise("distance_measurements", 1); // Subscribers vehicle_global_position_sub_ = @@ -76,9 +77,8 @@ AdaptiveSnowSampler::AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::N 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( - // "/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/cmd/command"); mavcmd_int_service_client_ = nh_.serviceClient("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 index bcad9ce..2ace3d8 100644 --- 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,7 +126,7 @@ Visualization Manager: Marker Topic: /home_position Name: Marker Namespaces: - {} + sphere: true Queue Size: 100 Value: true - Class: rviz/Marker @@ -133,7 +134,7 @@ Visualization Manager: Marker Topic: /setpoint_position Name: Marker Namespaces: - {} + sphere: true Queue Size: 100 Value: true - Class: rviz/Marker @@ -141,7 +142,7 @@ Visualization Manager: Marker Topic: /target_normal Name: Marker Namespaces: - {} + arrow: true Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -149,7 +150,16 @@ Visualization Manager: 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,7 +190,7 @@ 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 @@ -188,27 +198,27 @@ Visualization Manager: 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: - 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