Skip to content

Commit

Permalink
[Feature] Added support for offline foxglove visualizations
Browse files Browse the repository at this point in the history
  • Loading branch information
@artzha
artzha committed Dec 20, 2024
1 parent 819f978 commit 2a7967a
Show file tree
Hide file tree
Showing 8 changed files with 313 additions and 168 deletions.
35 changes: 2 additions & 33 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ LIST(APPEND CMAKE_PREFIX_PATH "/opt/libtorch")
FIND_PACKAGE(Torch REQUIRED)
FIND_PACKAGE(OpenCV REQUIRED)
FIND_PACKAGE(LibOSRM REQUIRED)
FIND_PACKAGE(foxglove_msgs REQUIRED)

IF(NOT ROS_BUILD_TYPE)
SET(ROS_BUILD_TYPE Release)
Expand Down Expand Up @@ -96,36 +97,4 @@ TARGET_LINK_LIBRARIES(streetmap_test
${LibOSRM_LIBRARIES}
${LibOSRM_DEPENDENT_LIBRARIES})

# ADD_EXECUTABLE(navigation_tests
# src/navigation/navigation_tests.cc
# src/navigation/motion_primitives.cc
# )
# TARGET_LINK_LIBRARIES(navigation_tests
# gtest
# gtest_main
# ${libs}
# ${LibOSRM_LIBRARIES}
# ${LibOSRM_DEPENDENT_LIBRARIES})

# ROSBUILD_ADD_EXECUTABLE(social_nav
# src/navigation/social_nav.cc
# src/navigation/social_main.cc
# src/navigation/motion_primitives.cc
# src/navigation/ackermann_motion_primitives.cc
# src/navigation/constant_curvature_arcs.cc
# src/navigation/image_tiler.cc
# src/navigation/linear_evaluator.cc
# src/navigation/deep_cost_map_evaluator.cc
# src/navigation/image_based_evaluator.cc
# src/navigation/navigation.cc)
# TARGET_LINK_LIBRARIES(social_nav
# shared_library
# ${libs}
# ${TORCH_LIBRARIES}
# ${OpenCV_LIBS}
# ${LibOSRM_LIBRARIES}
# ${LibOSRM_DEPENDENT_LIBRARIES})


add_dependencies(navigation ${catkin_EXPORTED_TARGETS})
# add_dependencies(social_nav ${catkin_EXPORTED_TARGETS})
add_dependencies(navigation ${catkin_EXPORTED_TARGETS})
2 changes: 1 addition & 1 deletion config/navigation.lua
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,4 +78,4 @@ AckermannSampler = {
-- distance_weight = 1.0;
-- free_path_weight = 1.0;
-- clearance_weight = 1.0;
-- }
-- }
1 change: 1 addition & 0 deletions manifest.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<depend package="amrl_msgs"/>
<depend package="cv_bridge"/>
<depend package="image_transport"/>
<!-- <depend package="foxglove_msgs"/> -->

</package>

Expand Down
137 changes: 34 additions & 103 deletions src/navigation/navigation.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,95 +244,6 @@ void Navigation::InitializeOSM(const OSMPlannerParameters& params) {
osm_planner_ = OSMPlanner(params.osrm_file, params.osrm_path_resolution);
}

// void Navigation::LoadVectorMap(
// const string& map_file) { // Assume map is given as MAP.navigation.json

// std::string vector_map_file = map_file;

// // Find the position of ".navigation.json"
// size_t found = vector_map_file.find(".navigation.json");

// // Replace ".navigation.json" with ".vectormap.json"
// if (found != std::string::npos) {
// vector_map_file.replace(found, std::string(".navigation.json").length(),
// ".vectormap.json");
// }

// // Output the modified string
// std::cout << "Loading vectormap file: " << vector_map_file << std::endl;

// int x_max = global_costmap_.getSizeInCellsX();
// int y_max = global_costmap_.getSizeInCellsY();
// global_costmap_.resetMap(0, 0, x_max, y_max);
// global_costmap_obstacles_.clear();

// std::ifstream i(vector_map_file);
// json j;
// i >> j;
// i.close();

// unordered_map<uint32_t, unsigned char> inflation_cells;

// for (const auto& line : j) {
// // Access specific fields in each dictionary
// Vector2f p0(line["p0"]["x"], line["p0"]["y"]);
// Vector2f p1(line["p1"]["x"], line["p1"]["y"]);

// float length = (p0 - p1).norm();
// for (float i = 0; i < length; i += params_.global_costmap_resolution) {
// Vector2f costmap_point = p0 + i * (p1 - p0) / length;
// uint32_t unsigned_mx, unsigned_my;
// bool in_map = global_costmap_.worldToMap(
// costmap_point.x(), costmap_point.y(), unsigned_mx, unsigned_my);
// if (in_map) {
// int cell_inflation_size = std::ceil(params_.max_inflation_radius /
// global_costmap_.getResolution());
// int mx = static_cast<int>(unsigned_mx);
// int my = static_cast<int>(unsigned_my);
// for (int j = -cell_inflation_size; j <= cell_inflation_size; j++) {
// for (int k = -cell_inflation_size; k <= cell_inflation_size; k++) {
// float cell_dist = sqrt(pow(j, 2) + pow(k, 2));
// float dist = cell_dist * global_costmap_.getResolution();
// if ((cell_dist <= cell_inflation_size) && (mx + j >= 0) &&
// (mx + j < x_max) && (my + k >= 0) && (my + k < y_max)) {
// unsigned char cost;
// if (j == 0 && k == 0) {
// cost = costmap_2d::LETHAL_OBSTACLE;
// } else if (dist <= params_.min_inflation_radius) {
// cost = costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
// } else {
// cost =
// std::ceil(std::exp(-1 * params_.inflation_coeff *
// (dist - params_.min_inflation_radius))
// *
// (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1));
// }
// global_costmap_.setCost(
// mx + j, my + k,
// std::max(cost, global_costmap_.getCost(mx + j, my + k)));
// inflation_cells[global_costmap_.getIndex(mx + j, my + k)] =
// std::max(cost, global_costmap_.getCost(mx + j, my + k));
// }
// }
// }
// }
// }
// }

// for (const auto& pair : inflation_cells) {
// auto index = pair.first;
// uint32_t mx = 0;
// uint32_t my = 0;
// global_costmap_.indexToCells(index, mx, my);
// // global_costmap_.setCost(mx, my, costmap_2d::LETHAL_OBSTACLE);

// double wx, wy;
// global_costmap_.mapToWorld(mx, my, wx, wy);
// global_costmap_obstacles_.push_back(
// ObstacleCost{Vector2f(wx, wy), pair.second});
// }
// }

bool Navigation::Enabled() const { return enabled_; }

void Navigation::Enable(bool enable) { enabled_ = enable; }
Expand Down Expand Up @@ -479,6 +390,17 @@ void Navigation::GetCompensatedOdomUTMTransform(Affine2f& T_tp_utm,
T_tp_utm = T_odom_utm * T_tp_odom;
}

bool Navigation::GetInitialOdom(Odom& odom) const {
if (!odom_initialized_) return false;
odom = initial_odom_msg_;
return true;
}
bool Navigation::GetInitialGPS(GPSPoint& loc) const {
if (!gps_initialized_) return false;
loc = initial_gps_loc_;
return true;
}

Affine2f Navigation::OdometryToUTMTransform(
const Odom& odom, const GPSPoint& gps_loc) { // Computes T^odom_utm
// Assumes that odom and gps_loc correspond to the same time
Expand Down Expand Up @@ -958,7 +880,7 @@ void Navigation::GPSPlannerTest(Vector2f& cmd_vel, float& cmd_angle_vel) {
robot_angle_);
printf("GPSPlannerTest(): %f %f\n", nav_goal_loc_.x(), nav_goal_loc_.y());
Vector2f carrot;
GetLocalCarrotHeading(carrot);
GetLocalCarrotHeading(carrot, true);

// Transform carrot to local frame
local_target_ = Rotation2Df(-robot_angle_) * (carrot - robot_loc_);
Expand Down Expand Up @@ -1039,7 +961,9 @@ Vector2f Navigation::GetPathGoal(float target_distance) {
bool Navigation::GetGlobalCarrot(Vector2f& carrot) {
for (float carrot_dist = params_.intermediate_goal_tolerance;
carrot_dist > params_.carrot_dist; carrot_dist -= 0.5) {
if (GetCarrot(carrot, true, carrot_dist)) {
// if (GetCarrot(carrot, true, carrot_dist)) {
if (GetLocalCarrotHeading(carrot, true)) {
printf("GetGlobalCarrot() %f %f\n", carrot.x(), carrot.y());
Vector2f robot_frame_carrot = carrot - robot_loc_;
uint32_t mx, my;
bool in_map = costmap_.worldToMap(robot_frame_carrot.x(),
Expand All @@ -1054,7 +978,14 @@ bool Navigation::GetGlobalCarrot(Vector2f& carrot) {
return false;
}

bool Navigation::GetLocalCarrotHeading(Vector2f& carrot) {
bool Navigation::GetGlobalPlan(std::vector<GPSPoint>& plan) const {
if (!gps_nav_goals_loc_.empty()) return false;

plan = gps_nav_goals_loc_;
return true;
}

bool Navigation::GetLocalCarrotHeading(Vector2f& carrot, bool global) {
if (gps_nav_goals_loc_.empty()) return false;
if (gps_goal_index_ < 0 || gps_goal_index_ >= int(gps_nav_goals_loc_.size()))
return false;
Expand All @@ -1064,26 +995,26 @@ bool Navigation::GetLocalCarrotHeading(Vector2f& carrot) {

// Goal carrot is a point on the carrot circle around robot
Vector2f goal_vec_norm = (T_goal_map - T_base_map).normalized();
// carrot = T_odom_map.translation() + goal_vec_norm * params_.carrot_dist;
Vector2f local_carrot = goal_vec_norm * params_.carrot_dist;
// Correct for heading to align with local frame
local_carrot = Rotation2Df(-robot_angle_) * local_carrot;

if (carrot_planner_ != nullptr) {
// Correct for heading to align with local frame
local_carrot = Rotation2Df(-robot_angle_) * local_carrot;

const CarrotPlan& plan =
carrot_planner_->GetCarrot(local_carrot, latest_odom_msg_);
if (plan.path.empty()) return false;
local_carrot =
plan.path[plan.path_idx]; // override local carrot with service planner

// Rotate back to global frame
local_carrot = Rotation2Df(robot_angle_) * local_carrot;
local_carrot = plan.path[plan.path_idx]; // override local carrot with service planner

printf("GetLocalCarrotHeading(): Global carrot from planner %f %f\n",
local_carrot.x(), local_carrot.y());
}
carrot = T_base_map + local_carrot; // Transform carrot to global frame
if (global) {
// Rotate back to global frame
local_carrot = Rotation2Df(robot_angle_) * local_carrot;
carrot = T_base_map + local_carrot; // Transform carrot to global frame
} else {
carrot = local_carrot;
}

return true;
}
Expand Down Expand Up @@ -1863,7 +1794,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,

/** Set local target */
Vector2f carrot(0, 0);
bool foundCarrot = GetLocalCarrotHeading(carrot);
bool foundCarrot = GetLocalCarrotHeading(carrot, true);
printf("Local carrot %f %f\n", carrot.x(), carrot.y());
printf("Next GPS goal %f %f\n", nav_goal_loc_.x(), nav_goal_loc_.y());
printf("Current Robot loc %f %f\n", robot_loc_.x(), robot_loc_.y());
Expand Down
5 changes: 4 additions & 1 deletion src/navigation/navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,7 @@ class Navigation {
const Eigen::Vector2f& end);
std::vector<GPSPoint> GlobalPlan(const GPSPoint& inital,
const std::vector<GPSPoint>& goals);
bool GetGlobalPlan(std::vector<GPSPoint>& plan) const;
std::vector<GraphDomain::State> GetPlanPath();
std::vector<GraphDomain::State> GetGlobalPath();

Expand All @@ -131,7 +132,7 @@ class Navigation {
Eigen::Vector2f GetPathGoal(float target_distance);
bool GetGlobalCarrot(Eigen::Vector2f& carrot);
bool GetLocalCarrot(Eigen::Vector2f& carrot);
bool GetLocalCarrotHeading(Eigen::Vector2f& carrot);
bool GetLocalCarrotHeading(Eigen::Vector2f& carrot, bool global);
bool GetCarrot(Eigen::Vector2f& carrot, bool global, float carrot_dist);
// Enable or disable autonomy.
void Enable(bool enable);
Expand Down Expand Up @@ -176,6 +177,8 @@ class Navigation {
std::shared_ptr<motion_primitives::PathRolloutBase> GetOption();
std::vector<ObstacleCost> GetCostmapObstacles();
std::vector<ObstacleCost> GetGlobalCostmapObstacles();
bool GetInitialOdom(Odom& odom) const;
bool GetInitialGPS(GPSPoint& loc) const;

Eigen::Vector2f GetIntermediateGoal();
// Get the next best global gps goal
Expand Down
Loading

0 comments on commit 2a7967a

Please sign in to comment.