diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h b/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h index e808746..b448d87 100644 --- a/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h +++ b/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 positionhistory_vector_; std::vector colored_trajectory_; Eigen::Quaterniond vehicle_attitude_{Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; diff --git a/adaptive_snowsampler/launch/replay_bag.launch b/adaptive_snowsampler/launch/replay_bag.launch index 3bae80f..9fa729d 100644 --- a/adaptive_snowsampler/launch/replay_bag.launch +++ b/adaptive_snowsampler/launch/replay_bag.launch @@ -1,6 +1,6 @@ - - + + diff --git a/adaptive_snowsampler/src/adaptive_snowsampler.cpp b/adaptive_snowsampler/src/adaptive_snowsampler.cpp index 7ccd6bb..e444368 100644 --- a/adaptive_snowsampler/src/adaptive_snowsampler.cpp +++ b/adaptive_snowsampler/src/adaptive_snowsampler.cpp @@ -263,7 +263,6 @@ 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(epsg); @@ -651,6 +650,7 @@ void AdaptiveSnowSampler::publishColoredTrajectory(const ros::Publisher &pub, co } } + std::cout << "max altitude: " << max_altitude << ", min altitude: " << min_altitude << std::endl; visualization_msgs::Marker msg; msg.header.frame_id = "map";