Fix replay

adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h

@@ -227,7 +227,8 @@ 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_;
   Eigen::Quaterniond vehicle_attitude_{Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
   boost::circular_buffer<Eigen::Vector3d> vehicle_attitude_buffer_{20};

adaptive_snowsampler/launch/replay_bag.launch

@@ -1,4 +1,5 @@
 <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"/>

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

@@ -139,9 +139,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 +262,7 @@ void AdaptiveSnowSampler::publishMap() {
   Eigen::Vector3d map_origin;
   map_->getGlobalOrigin(epsg, map_origin);
   map_origin_ = map_origin;
-
+  std::cout << "map_origin: " << map_origin_.transpose() << std::endl;
   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,9 @@ 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_);
+  }
 }
 // void AdaptiveSnowSampler::distanceSensorCallback(const px4_msgs::DistanceSensor &msg) {
 //   lidar_distance_ = msg.current_distance;

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