diff --git a/CMakeLists.txt b/CMakeLists.txt
index 873d54d..e31986d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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)
@@ -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})
\ No newline at end of file
diff --git a/config/navigation.lua b/config/navigation.lua
index c7e71c9..1d09a1c 100644
--- a/config/navigation.lua
+++ b/config/navigation.lua
@@ -78,4 +78,4 @@ AckermannSampler = {
-- distance_weight = 1.0;
-- free_path_weight = 1.0;
-- clearance_weight = 1.0;
--- }
\ No newline at end of file
+-- }
diff --git a/manifest.xml b/manifest.xml
index 4062633..5e43fdb 100644
--- a/manifest.xml
+++ b/manifest.xml
@@ -19,6 +19,7 @@
+
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
index d54009d..f72904b 100644
--- a/src/navigation/navigation.cc
+++ b/src/navigation/navigation.cc
@@ -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 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(unsigned_mx);
-// int my = static_cast(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; }
@@ -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
@@ -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_);
@@ -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(),
@@ -1054,7 +978,14 @@ bool Navigation::GetGlobalCarrot(Vector2f& carrot) {
return false;
}
-bool Navigation::GetLocalCarrotHeading(Vector2f& carrot) {
+bool Navigation::GetGlobalPlan(std::vector& 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;
@@ -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;
}
@@ -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());
diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h
index 283e6ce..9c60dfe 100644
--- a/src/navigation/navigation.h
+++ b/src/navigation/navigation.h
@@ -122,6 +122,7 @@ class Navigation {
const Eigen::Vector2f& end);
std::vector GlobalPlan(const GPSPoint& inital,
const std::vector& goals);
+ bool GetGlobalPlan(std::vector& plan) const;
std::vector GetPlanPath();
std::vector GetGlobalPath();
@@ -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);
@@ -176,6 +177,8 @@ class Navigation {
std::shared_ptr GetOption();
std::vector GetCostmapObstacles();
std::vector GetGlobalCostmapObstacles();
+ bool GetInitialOdom(Odom& odom) const;
+ bool GetInitialGPS(GPSPoint& loc) const;
Eigen::Vector2f GetIntermediateGoal();
// Get the next best global gps goal
diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc
index dc0e7d6..83c73ed 100644
--- a/src/navigation/navigation_main.cc
+++ b/src/navigation/navigation_main.cc
@@ -53,6 +53,7 @@
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/TwistStamped.h"
+#include "geometry_msgs/TransformStamped.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "graph_navigation/graphNavSrv.h"
@@ -76,10 +77,14 @@
#include "tf/transform_broadcaster.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
+
+#include "visualization/ros_visualization.h"
#include "visualization/visualization.h"
-#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
+// Foxglove includes
+#include "foxglove_msgs/GeoJSON.h"
+
using amrl_msgs::AckermannCurvatureDriveMsg;
using amrl_msgs::GPSArrayMsg;
using amrl_msgs::GPSMsg;
@@ -93,6 +98,8 @@ using Eigen::Vector2f;
using Eigen::Vector3f;
using geometry::kEpsilon;
using geometry_msgs::TwistStamped;
+using geometry_msgs::TransformStamped;
+using geometry_msgs::PoseStamped;
using graph_navigation::graphNavSrv;
using math_util::DegToRad;
using math_util::RadToDeg;
@@ -112,6 +119,8 @@ using sensor_msgs::PointCloud;
using std::string;
using std::unordered_map;
using std::vector;
+using visualization_msgs::MarkerArray;
+using foxglove_msgs::GeoJSON;
const string kAmrlMapsDir = ros::package::getPath("amrl_maps");
const string kOpenCVWindow = "Image window";
@@ -180,6 +189,10 @@ ros::Publisher localization_pub_;
ros::Publisher mission_status_pub_;
image_transport::Publisher viz_img_pub_;
+// ROS Publishers
+ros::Publisher geojson_pub_; // GeoJSON publisher
+ros::Publisher fox_path_pub_;
+
// Messages
visualization_msgs::Marker line_list_marker_;
visualization_msgs::Marker pose_marker_;
@@ -340,20 +353,39 @@ void GoToCallbackAMRL(const amrl_msgs::Localization2DMsg& msg) {
navigation_.Resume();
}
-// void GoToGPSGoalCallback(const std_msgs::Float64MultiArray& msg) {
-// if (msg.data.size() % 2 != 0) {
-// printf("Invalid GPS goal message, not divisible by 2\n");
-// return;
-// }
-
-// vector goals;
-// for (size_t i = 0; i < msg.data.size(); i += 2) {
-// goals.emplace_back(GPSPoint(msg.data[i], msg.data[i + 1]));
-// printf("GPS Goal: (%lf,%lf)\n", goals.back().lat, goals.back().lon);
-// }
-// navigation_.SetGPSNavGoal(goals);
-// navigation_.Resume();
-// }
+void PublishTF() {
+ static tf::TransformBroadcaster tf_broadcaster_;
+ // Publish the transform from the map frame to the odom frame
+ Odom odom;
+ GPSPoint gps_loc;
+ if (!navigation_.GetInitialOdom(odom) || !navigation_.GetInitialGPS(gps_loc)) return;
+ const auto T_odom_map = navigation_.OdometryToUTMTransform(odom, gps_loc);
+
+ // Extract 2D translation (x, y) and rotation (theta) from the 2D transform
+ Eigen::Vector2f translation_2d = T_odom_map.translation().head<2>();
+ float theta = atan2(T_odom_map.linear()(1, 0), T_odom_map.linear()(0, 0));
+
+ // Construct a 3D TransformStamped message
+ geometry_msgs::TransformStamped transform_msg;
+ transform_msg.header.stamp = ros::Time::now();
+ transform_msg.header.frame_id = "map";
+ transform_msg.child_frame_id = "odom";
+
+ // Set the 3D translation
+ transform_msg.transform.translation.x = translation_2d.x();
+ transform_msg.transform.translation.y = translation_2d.y();
+ transform_msg.transform.translation.z = 0.0; // Assume z = 0 for 2D transform
+
+ // Convert 2D rotation (theta) to a quaternion
+ tf::Quaternion q = tf::createQuaternionFromYaw(theta);
+ transform_msg.transform.rotation.x = q.x();
+ transform_msg.transform.rotation.y = q.y();
+ transform_msg.transform.rotation.z = q.z();
+ transform_msg.transform.rotation.w = q.w();
+
+ // Broadcast the transform
+ tf_broadcaster_.sendTransform(transform_msg);
+}
void ResetNavGoalsCallback(const std_msgs::Empty& msg) {
printf("Resetting all nav goals.\n");
@@ -418,18 +450,6 @@ void SignalHandler(int) {
run_ = false;
}
-// void LocalizationCallback(const amrl_msgs::Localization2DMsg& msg) {
-// static string map = "";
-// if (FLAGS_v > 2) {
-// printf("Localization t=%f\n", GetWallTime());
-// }
-// navigation_.UpdateLocation(Vector2f(msg.pose.x, msg.pose.y),
-// msg.pose.theta); if (map != msg.map) {
-// map = msg.map;
-// navigation_.UpdateMap(navigation::GetMapPath(FLAGS_maps_dir, msg.map));
-// }
-// }
-
void HaltCallback(const std_msgs::Bool& msg) { navigation_.Pause(); }
AckermannCurvatureDriveMsg TwistToAckermann(
@@ -560,6 +580,20 @@ nav_msgs::Path CarrotToNavMsgsPath(const Vector2f& carrot) {
return carrotNav;
}
+PoseStamped CarrotToPoseStamped(const Vector2f& carrot) {
+ PoseStamped carrotPose;
+ carrotPose.header.stamp = ros::Time::now();
+ carrotPose.header.frame_id = "base_link";
+ carrotPose.pose.position.x = carrot.x();
+ carrotPose.pose.position.y = carrot.y();
+ carrotPose.pose.position.z = 0.0;
+ carrotPose.pose.orientation.x = 0;
+ carrotPose.pose.orientation.y = 0;
+ carrotPose.pose.orientation.z = 0;
+ carrotPose.pose.orientation.w = 1;
+ return carrotPose;
+}
+
void PublishLocalization() {
// Publishes robot pose
Eigen::Vector3f robot_pose;
@@ -607,9 +641,10 @@ void PublishPath() {
0xA86032, global_viz_msg_);
}
Vector2f carrot;
- bool foundCarrot = navigation_.GetLocalCarrot(carrot);
+ bool foundCarrot = navigation_.GetLocalCarrotHeading(carrot, false);
if (foundCarrot) {
- carrot_pub_.publish(CarrotToNavMsgsPath(carrot));
+ // carrot_pub_.publish(CarrotToNavMsgsPath(carrot));
+ carrot_pub_.publish(CarrotToPoseStamped(carrot));
}
bool foundGlobalCarrot = navigation_.GetGlobalCarrot(carrot);
@@ -701,6 +736,17 @@ void DrawPathOptions() {
visualization::DrawPathOption(o.curvature, o.free_path_length, o.clearance,
0x0000FF, false, local_viz_msg_);
}
+
+ // Push best path option to the front of the vector
+ if (best_option != nullptr) {
+ path_options.insert(path_options.begin(), ToOptions({best_option})[0]);
+ }
+
+ // Create vector of colors for each path option (blue for all except best)
+ vector > colors(path_options.size(), {0.0, 0.0, 1.0, 1.0});
+ colors[0] = {1.0, 0.0, 0.0, 1.0};
+ ros_visualization::PathOptionToMarkerArray(fox_path_pub_, "base_link", path_options, colors, false);
+
if (best_option != nullptr) {
const ConstantCurvatureArc best_arc =
*reinterpret_cast(best_option.get());
@@ -710,6 +756,14 @@ void DrawPathOptions() {
}
}
+void PublishGlobalPlan() {
+ vector plan;
+ bool is_plan_valid = navigation_.GetGlobalPlan(plan);
+ if (is_plan_valid) {
+ ros_visualization::GPSRouteToGeoJSON(geojson_pub_, plan);
+ }
+}
+
/**
* Helper method that initializes visualization_msgs::Marker parameters
* @param vizMarker pointer to the visualization_msgs::Marker object
@@ -1059,10 +1113,14 @@ int main(int argc, char** argv) {
viz_img_pub_ = it_.advertise("vis_image", 1);
fp_pcl_pub_ = n.advertise("forward_predicted_pcl", 1);
path_pub_ = n.advertise("trajectory", 1);
- carrot_pub_ = n.advertise("carrot", 1, true);
+ carrot_pub_ = n.advertise("carrot", 1, true);
next_gps_goal_pub_ = n.advertise(
"next_gps_goal", 1, true); // Only publish if there is a goal
localization_pub_ = n.advertise("localization", 1);
+ geojson_pub_ = n.advertise("navigation/geojson_waypoints", 10);
+
+ // ROS path publishers
+ fox_path_pub_ = n.advertise("/navigation/path_rollouts", 1);
// Messages
local_viz_msg_ =
@@ -1130,9 +1188,11 @@ int main(int argc, char** argv) {
navigation_.Run(ros::Time::now().toSec(), cmd_vel, cmd_angle_vel);
// Publish Nav Status
+ PublishTF();
PublishNavStatus();
PublishMissionStatus();
PublishLocalization();
+ PublishGlobalPlan();
if (nav_succeeded) {
if (!FLAGS_no_intermed) {
// Publish Visualizations
diff --git a/src/navigation/navigation_types.h b/src/navigation/navigation_types.h
index 913f993..fb36f5a 100644
--- a/src/navigation/navigation_types.h
+++ b/src/navigation/navigation_types.h
@@ -18,6 +18,23 @@ struct PathOption {
Eigen::Vector2f obstruction;
Eigen::Vector2f closest_point;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ // Define an equality operator
+ bool operator==(const PathOption& other) const {
+ const float epsilon = 1e-6; // Tolerance for floating-point comparison
+ return std::fabs(curvature - other.curvature) < epsilon &&
+ std::fabs(clearance - other.clearance) < epsilon &&
+ std::fabs(free_path_length - other.free_path_length) < epsilon &&
+ std::fabs(clearance_to_goal - other.clearance_to_goal) < epsilon &&
+ std::fabs(dist_to_goal - other.dist_to_goal) < epsilon &&
+ (obstruction - other.obstruction).norm() < epsilon &&
+ (closest_point - other.closest_point).norm() < epsilon;
+ }
+
+ // Define an inequality operator (optional, for convenience)
+ bool operator!=(const PathOption& other) const {
+ return !(*this == other);
+ }
};
struct Twist {
diff --git a/src/visualization/ros_visualization.h b/src/visualization/ros_visualization.h
new file mode 100644
index 0000000..9e1defd
--- /dev/null
+++ b/src/visualization/ros_visualization.h
@@ -0,0 +1,164 @@
+#ifndef ROS_VISUALIZATION_H
+#define ROS_VISUALIZATION_H
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include "navigation/navigation_types.h"
+#include "shared/math/gps_util.h"
+
+// json support for geojson
+#include
+#include "navigation/json.hpp"
+
+using gps_util::GPSPoint;
+using json = nlohmann::json;
+using navigation::PathOption;
+
+namespace ros_visualization {
+
+void PathOptionToMarkerArray(
+ ros::Publisher& marker_array_pub,
+ const std::string& frame_id,
+ const std::vector& path_options,
+ const std::vector >& colors,
+ bool show_clearance = false) {
+
+ visualization_msgs::MarkerArray marker_array;
+
+ for (size_t i = 0; i < path_options.size(); ++i) {
+ const auto& path_option = path_options[i];
+
+ // Create primary path marker
+ visualization_msgs::Marker path_marker;
+ path_marker.header.frame_id = frame_id;
+ path_marker.header.stamp = ros::Time::now();
+ path_marker.ns = "path_options";
+ path_marker.id = static_cast(i * 3); // Unique ID
+ path_marker.type = visualization_msgs::Marker::LINE_STRIP;
+ path_marker.action = visualization_msgs::Marker::ADD;
+ path_marker.scale.x = 0.05; // Line thickness
+ path_marker.color.r = colors[i][0];
+ path_marker.color.g = colors[i][1];
+ path_marker.color.b = colors[i][2];
+ path_marker.color.a = colors[i][3];
+
+ const int num_points = 100;
+ float radius = (fabs(path_option.curvature) > 1e-6) ? (1.0f / path_option.curvature) : std::numeric_limits::infinity();
+ float arc_angle = path_option.curvature * path_option.free_path_length;
+
+ for (int j = 0; j <= num_points; ++j) {
+ float theta = (arc_angle / num_points) * j;
+ geometry_msgs::Point p;
+ if (fabs(path_option.curvature) < 1e-6) {
+ p.x = (path_option.free_path_length / num_points) * j;
+ p.y = 0.0;
+ } else {
+ p.x = radius * sin(theta);
+ p.y = radius * (1 - cos(theta));
+ }
+ p.z = 0.0;
+ path_marker.points.push_back(p);
+ }
+
+ marker_array.markers.push_back(path_marker);
+
+ if (show_clearance) {
+ // Draw clearance arcs
+ visualization_msgs::Marker clearance_inner_marker = path_marker;
+ visualization_msgs::Marker clearance_outer_marker = path_marker;
+
+ clearance_inner_marker.id = static_cast(i * 3 + 1);
+ clearance_outer_marker.id = static_cast(i * 3 + 2);
+
+ clearance_inner_marker.color.r = 0.5;
+ clearance_inner_marker.color.g = 0.5;
+ clearance_inner_marker.color.b = 0.5;
+ clearance_inner_marker.color.a = 0.5;
+
+ clearance_outer_marker.color.r = 0.5;
+ clearance_outer_marker.color.g = 0.5;
+ clearance_outer_marker.color.b = 0.5;
+ clearance_outer_marker.color.a = 0.5;
+
+ clearance_inner_marker.points.clear();
+ clearance_outer_marker.points.clear();
+
+ for (int j = 0; j <= num_points; ++j) {
+ float theta = (arc_angle / num_points) * j;
+ geometry_msgs::Point p_inner, p_outer;
+ if (fabs(path_option.curvature) < 1e-6) {
+ p_inner.x = (path_option.free_path_length / num_points) * j;
+ p_inner.y = -path_option.clearance;
+ p_outer.x = (path_option.free_path_length / num_points) * j;
+ p_outer.y = path_option.clearance;
+ } else {
+ p_inner.x = (radius - path_option.clearance) * sin(theta);
+ p_inner.y = (radius - path_option.clearance) * (1 - cos(theta));
+ p_outer.x = (radius + path_option.clearance) * sin(theta);
+ p_outer.y = (radius + path_option.clearance) * (1 - cos(theta));
+ }
+ p_inner.z = 0.0;
+ p_outer.z = 0.0;
+ clearance_inner_marker.points.push_back(p_inner);
+ clearance_outer_marker.points.push_back(p_outer);
+ }
+
+ marker_array.markers.push_back(clearance_inner_marker);
+ marker_array.markers.push_back(clearance_outer_marker);
+ }
+ }
+
+ marker_array_pub.publish(marker_array);
+}
+
+void GPSRouteToGeoJSON(ros::Publisher& geojson_pub, const std::vector& route) {
+ // Create GeoJSON message
+ foxglove_msgs::GeoJSON geojson_msg;
+
+ // Create the GeoJSON object
+ json geojson;
+ geojson["type"] = "FeatureCollection";
+ geojson["features"] = json::array();
+
+ // Create a single LineString feature
+ json feature;
+ feature["type"] = "Feature";
+
+ // Add properties (customizable as needed)
+ feature["properties"] = {
+ {"name", "Route"},
+ {"style", {
+ {"color", "#FF0000"}, // Green color
+ {"weight", 5}, // Line thickness
+ {"opacity", 1.0} // Line opacity
+ }}
+ };
+
+ // Add geometry for LineString
+ feature["geometry"] = {
+ {"type", "LineString"},
+ {"coordinates", json::array()}
+ };
+
+ // Populate coordinates array with route points
+ for (const auto& point : route) {
+ feature["geometry"]["coordinates"].push_back({point.lon, point.lat});
+ }
+
+ // Add the LineString feature to the GeoJSON
+ geojson["features"].push_back(feature);
+
+ // Serialize GeoJSON to string
+ geojson_msg.geojson = geojson.dump();
+
+ // Publish the GeoJSON message
+ geojson_pub.publish(geojson_msg);
+}
+
+} // namespace ros_visualization
+
+#endif // ROS_VISUALIZATION_H