[WIP] Flight replay

adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h

@@ -180,6 +180,9 @@ class AdaptiveSnowSampler {
                                                 const std::string marker_namespace = "sphere");
   geometry_msgs::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation);
 
+  void publishColoredTrajectory(const ros::Publisher &pub, const Eigen::Vector3d &vehicle_position,
+                                                   std::vector<Eigen::Vector3d> &position_history);
+
   ros::NodeHandle nh_;
   ros::NodeHandle nh_private_;
 
@@ -204,6 +207,7 @@ class AdaptiveSnowSampler {
   ros::Publisher snow_depth_pub_;
   ros::Publisher vehicle_pose_pub_;
   ros::Publisher map_info_pub_;
+  ros::Publisher coloredhistory_pub_;
 
   ros::Subscriber vehicle_attitude_sub_;
   ros::Subscriber vehicle_global_position_sub_;
@@ -227,8 +231,10 @@ class AdaptiveSnowSampler {
 
   Eigen::Vector3d vehicle_position_{Eigen::Vector3d(0.0, 0.0, 0.0)};
   Eigen::Vector3d lv03_vehicle_position_{Eigen::Vector3d(0.0, 0.0, 0.0)};
-  Eigen::Vector3d map_origin_{Eigen::Vector3d{0.0, 0.0, 0.0}};
+  // Eigen::Vector3d map_origin_{Eigen::Vector3d{787802.0, 185985.0, 0.0}};  // Fluelatal
+  Eigen::Vector3d map_origin_{Eigen::Vector3d{783936.0, 184512.0, 0.0}}; //Braemabuel
   std::vector<geometry_msgs::PoseStamped> positionhistory_vector_;
+  std::vector<Eigen::Vector3d> colored_trajectory_;
   Eigen::Quaterniond vehicle_attitude_{Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
   boost::circular_buffer<Eigen::Vector3d> vehicle_attitude_buffer_{20};
   Eigen::Vector3d vehicle_attitude_filtered_ref_{Eigen::Vector3d(0.0, 0.0, 0.0)};

adaptive_snowsampler/launch/replay_bag.launch

@@ -1,15 +1,12 @@
 <launch>
+    <!-- <arg name="location" default="flueelatal"/> -->
     <arg name="location" default="braemabuel_1m_crop"/>
     <arg name="path" default="$(find adaptive_snowsampler)/scripts/file.bag"/>
     <arg name="visualization" default="true"/>
     <arg name="start_time" default="0.0"/>
     <arg name="duration" default="1500.0"/>
     <node pkg="tf" type="static_transform_publisher" name="world_map" args="0 0 0 0 0 0 world map 10"/>
 
-    <group if="$(arg visualization)">
-        <node type="rviz" name="rviz" pkg="rviz" args="-d $(find snowsampler_rviz)/launch/config.rviz" output="screen"/>
-    </group>
-
     <node pkg="adaptive_snowsampler" type="adaptive_snowsampler" name="snowsampler_node" output="screen">
         <param name="tif_path" value="$(find adaptive_snowsampler)/resources/$(arg location).tif"/>
         <param name="tif_color_path" value="$(find adaptive_snowsampler)/resources/$(arg location)_color.tif"/>
@@ -18,4 +15,7 @@
     <node pkg="rosbag" type="play" name="player" args="$(arg path) 
     --clock --start $(arg start_time) --duration $(arg duration) -r 10"/>
 
+    <group if="$(arg visualization)">
+        <node type="rviz" name="rviz" pkg="rviz" args="-d $(find snowsampler_rviz)/launch/replay.rviz" output="screen"/>
+    </group>
 </launch>

adaptive_snowsampler/scripts/log_replay.py

@@ -110,6 +110,8 @@ def convert_ulog2rosbag(path, rosbag_file_name, messages, disable_str_exceptions
             list.sort()
 
             for filename in list:
+                if os.path.isdir(os.path.join(path, filename)):
+                    continue
                 print(filename)
                 appendBag(os.path.join(path, filename), bag)
         else:

adaptive_snowsampler/src/adaptive_snowsampler.cpp

@@ -69,6 +69,7 @@ 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);
+  coloredhistory_pub_ = nh_.advertise<visualization_msgs::Marker>("coloredhistory", 1);
 
   // Subscribers
   vehicle_global_position_sub_ =
@@ -139,9 +140,6 @@ AdaptiveSnowSampler::AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::N
 }
 
 void AdaptiveSnowSampler::cmdloopCallback(const ros::TimerEvent &event) {
-  if (global_position_received_) {
-    publishPositionHistory(referencehistory_pub_, vehicle_position_, positionhistory_vector_);
-  }
   publishVehiclePose(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_);
 }
 
@@ -265,7 +263,6 @@ void AdaptiveSnowSampler::publishMap() {
   Eigen::Vector3d map_origin;
   map_->getGlobalOrigin(epsg, map_origin);
   map_origin_ = map_origin;
-
   grid_map_geo_msgs::GeographicMapInfo map_info_msg;
   map_info_msg.header.stamp = ros::Time::now();
   map_info_msg.geo_coordinate = static_cast<int>(epsg);
@@ -340,6 +337,10 @@ void AdaptiveSnowSampler::vehicleGlobalPositionCallback(const sensor_msgs::NavSa
 
   // Send the transformation
   // tf_broadcaster_->sendTransform(t);
+  if (global_position_received_) {
+    publishPositionHistory(referencehistory_pub_, vehicle_position_, positionhistory_vector_);
+    publishColoredTrajectory(coloredhistory_pub_, vehicle_position_, colored_trajectory_);
+  }
 }
 // void AdaptiveSnowSampler::distanceSensorCallback(const px4_msgs::DistanceSensor &msg) {
 //   lidar_distance_ = msg.current_distance;
@@ -623,6 +624,70 @@ void AdaptiveSnowSampler::publishPositionHistory(const ros::Publisher &pub, cons
   pub.publish(msg);
 }
 
+void AdaptiveSnowSampler::publishColoredTrajectory(const ros::Publisher &pub, const Eigen::Vector3d &vehicle_position,
+                                                   std::vector<Eigen::Vector3d> &position_history) {
+
+    unsigned int posehistory_window_ = 4000;
+    position_history.push_back(vehicle_position);
+    if (position_history.size() > posehistory_window_) {
+      position_history.pop_back();
+    }
+
+
+    const std::vector<std::vector<float>>& ctable = colorMap.at("magma");
+
+    std::vector<Eigen::Vector3d> segment_colors;
+
+    //Identify limits of trajectory
+    double min_altitude{std::numeric_limits<double>::infinity()};
+    double max_altitude{-std::numeric_limits<double>::infinity()};
+    for (auto& position : position_history) {
+      if (position(2) > max_altitude) {
+        max_altitude = position(2);
+      }
+      if (position(2) < min_altitude) {
+        min_altitude = position(2);
+      }
+    }
+
+    std::cout << "max altitude: " << max_altitude << ", min altitude: " << min_altitude << std::endl;
+
+    visualization_msgs::Marker msg;
+    msg.header.frame_id = "map";
+    msg.header.stamp = ros::Time::now();
+    msg.id = 0;
+    msg.type = visualization_msgs::Marker::LINE_STRIP;
+    msg.action = visualization_msgs::Marker::ADD;
+    msg.pose.orientation.x = 0.0;
+    msg.pose.orientation.y = 0.0;
+    msg.pose.orientation.z = 0.0;
+    msg.pose.orientation.w = 1.0;
+    msg.scale.x = 2.0;
+    msg.scale.y = 2.0;
+    msg.scale.z = 2.0;
+    for (auto& position : position_history) {
+      double intensity = (position(2) - min_altitude) / (max_altitude - min_altitude);
+      // // std::cout << "intensity: " << intensity << " segment_id: " << segment_id << std::endl;
+      intensity = std::max(std::min(intensity, 1.0), 0.0);
+      int idx = std::min(std::max(int(floor(intensity * 255.0)), 0), 255);
+      // Get color from table
+      std::vector<float> rgb = ctable.at(idx);
+      std_msgs::ColorRGBA color;
+      color.r = static_cast<double>(rgb[0]);
+      color.g = static_cast<double>(rgb[1]);
+      color.b = static_cast<double>(rgb[2]);
+      color.a = 1.0;
+      // Eigen::Vector3d segment_color();
+      msg.colors.push_back(color);
+      geometry_msgs::Point point;
+      point.x = position(0);
+      point.y = position(1);
+      point.z = position(2);
+      msg.points.push_back(point);
+    }
+    pub.publish(msg);
+}
+
 void AdaptiveSnowSampler::callSetAngleService(double angle) {
   std::cout << "Calling service" << std::endl;
   std::string service_name = "snowsampler/set_landing_leg_angle";

snowsampler_rviz/launch/config.rviz

@@ -34,10 +34,6 @@ Panels:
     SyncSource: ""
   - Class: snowsampler_rviz/PlanningPanel
     Name: PlanningPanel
-    namespace: ""
-    odometry_topic: ""
-    planner_name: ""
-    planning_budget: 100.0
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -69,14 +65,11 @@ Visualization Manager:
       Color: 200; 200; 200
       Color Layer: color
       Color Transformer: ColorLayer
-      ColorMap: default
       Enabled: true
-      Grid Cell Decimation: 1
-      Grid Line Thickness: 0.10000000149011612
       Height Layer: elevation
       Height Transformer: GridMapLayer
       History Length: 1
-      Invert ColorMap: false
+      Invert Rainbow: false
       Max Color: 255; 255; 255
       Max Intensity: 10
       Min Color: 0; 0; 0
@@ -85,7 +78,7 @@ Visualization Manager:
       Show Grid Lines: false
       Topic: /elevation_map
       Unreliable: false
-      Use ColorMap: true
+      Use Rainbow: true
       Value: true
     - Alpha: 1
       Buffer Length: 1
@@ -125,31 +118,32 @@ 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
   Enabled: true
@@ -180,25 +174,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 242.64480590820312
+      Distance: 273.07568359375
       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: -73.24365234375
+        Y: 107.97415924072266
+        Z: 2238.623291015625
       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.2403990477323532
       Target Frame: <Fixed Frame>
-      Yaw: 5.393568515777588
+      Yaw: 1.540382981300354
     Saved: ~
 Window Geometry:
   Displays:
@@ -219,4 +213,4 @@ Window Geometry:
     collapsed: false
   Width: 1386
   X: 432
-  Y: 82
+  Y: 45