cyy/car_nav_lite
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

MPPI + fence-line loc + waypoint nav with fail skip

Browse Source
This commit is contained in:
cyy
2026-07-03 00:17:08 +00:00
parent 5156a8d766
commit 50ff61a16b
14 changed files with 555 additions and 115 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/main.cpp
View File

@@ -6,6 +6,7 @@
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <std_msgs/msg/string.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
@@ -69,6 +70,11 @@ public:
pub_cmd_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
pub_path_ = create_publisher<nav_msgs::msg::Path>("/global_path", 10);
pub_map_ = create_publisher<nav_msgs::msg::OccupancyGrid>("/map", rclcpp::QoS(1).transient_local());
pub_status_ = create_publisher<std_msgs::msg::String>("/nav_status", 10);
bt_.setFailCallback([this](){
std_msgs::msg::String msg; msg.data = "fail";
pub_status_->publish(msg);
});
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
if (!mf_.empty()) {
@@ -79,6 +85,7 @@ public:
timer_ = create_wall_timer(50ms, std::bind(&NavLiteNode::tick, this));
map_timer_ = create_wall_timer(1s, std::bind(&NavLiteNode::publish_map, this));
correct_timer_ = create_wall_timer(200ms, std::bind(&NavLiteNode::correct_localization, this));
morph_timer_ = create_wall_timer(100ms, std::bind(&NavLiteNode::morph_close, this));
RCLCPP_INFO(get_logger(),"car_nav_lite ready. test_mode:=true for auto path. "
"ros2 param set /nav_lite_node <name> <value>");
@@ -105,6 +112,7 @@ private:
// ── State ──
std::mutex mtx_;
LaserScan latest_scan_; bool has_scan_=false;
Pose2D scan_pose_snapshot_;
double latest_vx_=0, latest_wz_=0, latest_gyro_z_=0;
rclcpp::Time last_odom_t_{0,0,RCL_ROS_TIME};
rclcpp::Time last_imu_t_{0,0,RCL_ROS_TIME};
@@ -116,14 +124,16 @@ private:
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_cmd_;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_map_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_status_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_;
rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_timer_, morph_timer_;
void scan_cb(LaserScanMsg::SharedPtr m){std::lock_guard lk(mtx_);
latest_scan_.angle_min=m->angle_min;latest_scan_.angle_max=m->angle_max;
latest_scan_.angle_increment=m->angle_increment;
latest_scan_.range_min=m->range_min;latest_scan_.range_max=m->range_max;
latest_scan_.ranges=m->ranges;has_scan_=true;}
latest_scan_.ranges=m->ranges;has_scan_=true;
scan_pose_snapshot_ = localizer_.pose();}
void odom_cb(OdometryMsg::SharedPtr m){std::lock_guard lk(mtx_);
// EXACT-MPPI pattern: trust odom pose directly (sim = ground truth,
// real robot = wheel-encoder / EKF output). No manual integration.
@@ -229,11 +239,13 @@ private:
void correct_localization(){
if(!has_scan_)return;
auto pose_before = localizer_.pose();
localizer_.correctWithScanCV(latest_scan_, map_);
localizer_.correctWithScanCV(latest_scan_, map_, &scan_pose_snapshot_);
auto pose_after = localizer_.pose();
(void)pose_before; (void)pose_after;
}
void morph_close(){std::lock_guard lk(mtx_); map_.morphologyClose(3);}
void publish_tf(const Pose2D& pose){
geometry_msgs::msg::TransformStamped tf;
tf.header.stamp=now();tf.header.frame_id="map";tf.child_frame_id="base_link";