Colored path

Author: Jaeyoung-Lim

adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h

@@ -231,8 +231,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{787802.0, 185985.0, 0.0}};  // Fluelatal
-  Eigen::Vector3d map_origin_{Eigen::Vector3d{783936.0, 184512.0, 0.0}}; //Braemabuel
+  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)};

adaptive_snowsampler/launch/replay_bag.launch

@@ -1,6 +1,6 @@
 <launch>
-    <!-- <arg name="location" default="flueelatal"/> -->
-    <arg name="location" default="braemabuel_1m_crop"/>
+    <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"/>

adaptive_snowsampler/src/adaptive_snowsampler.cpp

@@ -628,7 +628,7 @@ void AdaptiveSnowSampler::publishPositionHistory(const ros::Publisher &pub, cons
 void AdaptiveSnowSampler::publishColoredTrajectory(const ros::Publisher &pub, const Eigen::Vector3d &vehicle_position,
                                                    std::vector<Eigen::Vector3d> &position_history) {
 
-    unsigned int posehistory_window_ = 2000;
+    unsigned int posehistory_window_ = 4000;
     position_history.push_back(vehicle_position);
     if (position_history.size() > posehistory_window_) {
       position_history.pop_back();