MPPI planner + fence-line localization + scan obstacles
- Lightweight MPPI (200 trajectories, 15 steps, endpoint path_align) - adaptive exploration (more random when stuck) - fence-line Hough detection + geometric correction - scan hit points marked at 255 immediately - forward half-step pre-check for obstacle avoidance - Ackermann constraint |wz| <= |vx|/min_turn_r - XORshift RNG (stdlib broken on ARM)
This commit is contained in:
cyy
245
src/main.cpp
Normal file
245
src/main.cpp
Normal file
@@ -0,0 +1,245 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <nav_msgs/msg/odometry.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <nav_msgs/msg/occupancy_grid.hpp>
|
||||
#include <sensor_msgs/msg/imu.hpp>
|
||||
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <geometry_msgs/msg/transform_stamped.hpp>
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
|
||||
#include "car_nav_lite/types.hpp"
|
||||
#include "car_nav_lite/grid_map.hpp"
|
||||
#include "car_nav_lite/localizer.hpp"
|
||||
#include "car_nav_lite/bt_executor.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
using namespace car_nav_lite;
|
||||
using LaserScanMsg = sensor_msgs::msg::LaserScan;
|
||||
using OdometryMsg = nav_msgs::msg::Odometry;
|
||||
using ImuMsg = sensor_msgs::msg::Imu;
|
||||
using PoseStMsg = geometry_msgs::msg::PoseStamped;
|
||||
|
||||
class NavLiteNode : public rclcpp::Node {
|
||||
public:
|
||||
NavLiteNode() : Node("nav_lite_node") {
|
||||
// ── ROS2 dynamic parameters ──
|
||||
declare_parameter("map_file", std::string(""));
|
||||
declare_parameter("test_mode", false);
|
||||
declare_parameter("lookahead", 0.5);
|
||||
declare_parameter("max_speed", 1.0);
|
||||
declare_parameter("min_turn_radius", 0.20);
|
||||
declare_parameter("steer_limit", 0.6);
|
||||
declare_parameter("speed_curv_factor", 6.0);
|
||||
_read_params();
|
||||
|
||||
param_cb_ = add_on_set_parameters_callback(
|
||||
[this](const std::vector<rclcpp::Parameter>& params) {
|
||||
for (auto& p : params) {
|
||||
std::string n = p.get_name();
|
||||
if (n=="lookahead") la_=p.as_double();
|
||||
else if (n=="max_speed") ms_=p.as_double();
|
||||
else if (n=="min_turn_radius") mr_=p.as_double();
|
||||
else if (n=="steer_limit") sl_=p.as_double();
|
||||
else if (n=="speed_curv_factor") cf_=p.as_double();
|
||||
else if (n=="test_mode") { test_=p.as_bool(); tps_=false; }
|
||||
}
|
||||
RCLCPP_INFO(get_logger(),"params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f test=%d",
|
||||
la_,ms_,mr_,sl_,cf_,test_);
|
||||
return rcl_interfaces::msg::SetParametersResult().set__successful(true);
|
||||
});
|
||||
|
||||
// ── Subscribers ──
|
||||
sub_scan_ = create_subscription<LaserScanMsg>("/scan", 10,
|
||||
std::bind(&NavLiteNode::scan_cb, this, std::placeholders::_1));
|
||||
sub_odom_ = create_subscription<OdometryMsg>("/odom", 10,
|
||||
std::bind(&NavLiteNode::odom_cb, this, std::placeholders::_1));
|
||||
sub_imu_ = create_subscription<ImuMsg>("/imu/data_raw", 10,
|
||||
std::bind(&NavLiteNode::imu_cb, this, std::placeholders::_1));
|
||||
sub_goal_ = create_subscription<PoseStMsg>("/goal_pose", 10,
|
||||
std::bind(&NavLiteNode::goal_cb, this, std::placeholders::_1));
|
||||
|
||||
// ── Publishers ──
|
||||
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());
|
||||
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
|
||||
|
||||
if (!mf_.empty()) {
|
||||
if (!map_.loadPGM(mf_)) RCLCPP_WARN(get_logger(),"Map load fail");
|
||||
else RCLCPP_INFO(get_logger(),"Map OK %dx%d",map_.width(),map_.height());
|
||||
}
|
||||
|
||||
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));
|
||||
|
||||
RCLCPP_INFO(get_logger(),"car_nav_lite ready. test_mode:=true for auto path. "
|
||||
"ros2 param set /nav_lite_node <name> <value>");
|
||||
RCLCPP_INFO(get_logger(),"Params: la=%.2f ms=%.1f mr=%.2f sl=%.2f cf=%.1f",
|
||||
la_,ms_,mr_,sl_,cf_);
|
||||
}
|
||||
|
||||
private:
|
||||
// ── Dynamic params ──
|
||||
std::string mf_;
|
||||
bool test_=false, tps_=false;
|
||||
double la_=0.5, ms_=1.0, mr_=0.20, sl_=0.6, cf_=6.0;
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
|
||||
void _read_params(){
|
||||
mf_=get_parameter("map_file").as_string();
|
||||
test_=get_parameter("test_mode").as_bool();
|
||||
la_=get_parameter("lookahead").as_double();
|
||||
ms_=get_parameter("max_speed").as_double();
|
||||
mr_=get_parameter("min_turn_radius").as_double();
|
||||
sl_=get_parameter("steer_limit").as_double();
|
||||
cf_=get_parameter("speed_curv_factor").as_double();
|
||||
}
|
||||
|
||||
// ── State ──
|
||||
std::mutex mtx_;
|
||||
LaserScan latest_scan_; bool has_scan_=false;
|
||||
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};
|
||||
GridMap map_; Localizer localizer_; BTExecutor bt_;
|
||||
rclcpp::Subscription<LaserScanMsg>::SharedPtr sub_scan_;
|
||||
rclcpp::Subscription<OdometryMsg>::SharedPtr sub_odom_;
|
||||
rclcpp::Subscription<ImuMsg>::SharedPtr sub_imu_;
|
||||
rclcpp::Subscription<PoseStMsg>::SharedPtr sub_goal_;
|
||||
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_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
rclcpp::TimerBase::SharedPtr timer_, map_timer_, correct_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;}
|
||||
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.
|
||||
auto& p = m->pose.pose;
|
||||
double qx=p.orientation.x, qy=p.orientation.y, qz=p.orientation.z, qw=p.orientation.w;
|
||||
double yaw = std::atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz));
|
||||
localizer_.setOdomPose({p.position.x, p.position.y, yaw});
|
||||
last_odom_t_ = rclcpp::Time(m->header.stamp);
|
||||
latest_vx_ = m->twist.twist.linear.x;
|
||||
latest_wz_ = m->twist.twist.angular.z;}
|
||||
void imu_cb(ImuMsg::SharedPtr m){std::lock_guard lk(mtx_);
|
||||
auto now=rclcpp::Time(m->header.stamp);
|
||||
double dt=last_imu_t_.nanoseconds()>0?(now-last_imu_t_).seconds():0.005;
|
||||
last_imu_t_=now;latest_gyro_z_=m->angular_velocity.z;
|
||||
if(dt>0&&dt<1.0)localizer_.updateIMU(latest_gyro_z_,dt);}
|
||||
void goal_cb(PoseStMsg::SharedPtr m){std::lock_guard lk(mtx_);
|
||||
if(m->header.frame_id!="map"){RCLCPP_WARN(get_logger(),"goal frame not map");return;}
|
||||
Pose2D g(m->pose.position.x,m->pose.position.y,0.0);
|
||||
double qw=m->pose.orientation.w,qz=m->pose.orientation.z;
|
||||
if(qw!=0||qz!=0)g.yaw=std::atan2(2.0*qw*qz,qw*qw-qz*qz);
|
||||
bt_.setGoal(g);tps_=false;}
|
||||
|
||||
// ── Test path: figure-8 with 30cm circles ──
|
||||
std::vector<Pose2D> _genTestPath(){
|
||||
std::vector<Pose2D> wp;
|
||||
double cx=0,cy=-0.5,r=0.15; // 15cm radius = 30cm diameter
|
||||
int N=30;
|
||||
// Circle 1: counter-clockwise
|
||||
for(int i=0;i<=N;++i){
|
||||
double a=2.0*M_PI*i/N;
|
||||
wp.push_back({cx-r*std::cos(a),cy-r+r*std::sin(a),0});
|
||||
}
|
||||
// Straight connecting segment
|
||||
wp.push_back({cx,cy-2*r,0});
|
||||
// Circle 2: clockwise (other direction), centered right
|
||||
for(int i=0;i<=N;++i){
|
||||
double a=-2.0*M_PI*i/N;
|
||||
wp.push_back({cx+r*std::cos(a),cy-3*r+r*std::sin(a),0});
|
||||
}
|
||||
// Straight back
|
||||
wp.push_back({cx,cy,0});
|
||||
// Rectangle 2x1.5m
|
||||
for(auto& p:std::vector<Pose2D>{{cx-1,cy-0.75,0},{cx+1,cy-0.75,0},{cx+1,cy+0.75,0},{cx-1,cy+0.75,0},{cx-1,cy-0.75,0}})
|
||||
wp.push_back(p);
|
||||
// Interpolate
|
||||
std::vector<Pose2D> dense;
|
||||
for(size_t i=0;i+1<wp.size();++i){
|
||||
double d=wp[i].distTo(wp[i+1]);
|
||||
int steps=std::max(1,(int)(d/0.05));
|
||||
for(int s=0;s<steps;++s){
|
||||
double t=(double)s/steps;
|
||||
dense.push_back({wp[i].x+t*(wp[i+1].x-wp[i].x),wp[i].y+t*(wp[i+1].y-wp[i].y),0});
|
||||
}
|
||||
}
|
||||
for(size_t i=0;i+1<dense.size();++i)dense[i].yaw=dense[i].angleTo(dense[i+1]);
|
||||
if(!dense.empty())dense.back().yaw=dense[dense.size()-2].yaw;
|
||||
return dense;
|
||||
}
|
||||
|
||||
void tick(){
|
||||
std::lock_guard lk(mtx_);
|
||||
auto pose=localizer_.pose();publish_tf(pose);
|
||||
if(has_scan_)map_.updateScan(latest_scan_,pose);
|
||||
|
||||
// Test mode: auto-activate figure-8 after 3s
|
||||
if(test_&&!tps_){
|
||||
static int tc=0;
|
||||
if(++tc>60){
|
||||
auto tp=_genTestPath();
|
||||
bt_.setTestPath(tp);tps_=true;
|
||||
RCLCPP_INFO(get_logger(),"Test path: fig8+rect, %zu pts",tp.size());
|
||||
}
|
||||
}
|
||||
|
||||
// Update BT with current params
|
||||
bt_.setParams(la_,ms_,mr_,sl_,cf_);
|
||||
Twist cmd=bt_.tick(pose,map_);
|
||||
|
||||
auto tw=geometry_msgs::msg::Twist();tw.linear.x=cmd.vx;tw.angular.z=cmd.wz;
|
||||
pub_cmd_->publish(tw);
|
||||
if(!bt_.currentPath().empty()){
|
||||
auto pm=nav_msgs::msg::Path();pm.header.stamp=now();pm.header.frame_id="map";
|
||||
for(auto&p:bt_.currentPath()){
|
||||
geometry_msgs::msg::PoseStamped ps;ps.header.frame_id="map";
|
||||
ps.pose.position.x=p.x;ps.pose.position.y=p.y;
|
||||
ps.pose.orientation.z=std::sin(p.yaw/2.0);ps.pose.orientation.w=std::cos(p.yaw/2.0);
|
||||
pm.poses.push_back(ps);
|
||||
}pub_path_->publish(pm);
|
||||
}
|
||||
static int ct=0;
|
||||
if(++ct%60==0)RCLCPP_INFO(get_logger(),"pose=(%.2f,%.2f,%.0f) cmd=(%.2f,%.2f)",
|
||||
pose.x,pose.y,pose.yaw*57.3,cmd.vx,cmd.wz);
|
||||
}
|
||||
|
||||
void publish_map(){std::lock_guard lk(mtx_);
|
||||
auto m=nav_msgs::msg::OccupancyGrid();m.header.stamp=now();m.header.frame_id="map";
|
||||
m.info.resolution=GRID_RES;m.info.width=m.info.height=GRID_SIZE;
|
||||
m.info.origin.position.x=GRID_ORIGIN_X;m.info.origin.position.y=GRID_ORIGIN_Y;
|
||||
m.data.resize(GRID_SIZE*GRID_SIZE);
|
||||
for(size_t i=0;i<map_.data().size();++i){uint8_t v=map_.data()[i];m.data[i]=(v==0)?0:((v>128)?100:-1);}
|
||||
pub_map_->publish(m);}
|
||||
|
||||
void correct_localization(){
|
||||
if(!has_scan_)return;
|
||||
auto pose_before = localizer_.pose();
|
||||
localizer_.correctWithScanCV(latest_scan_, map_);
|
||||
auto pose_after = localizer_.pose();
|
||||
(void)pose_before; (void)pose_after;
|
||||
}
|
||||
|
||||
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";
|
||||
tf.transform.translation.x=pose.x;tf.transform.translation.y=pose.y;
|
||||
double hy=pose.yaw/2.0;tf.transform.rotation.z=std::sin(hy);tf.transform.rotation.w=std::cos(hy);
|
||||
tf_broadcaster_->sendTransform(tf);}
|
||||
};
|
||||
|
||||
int main(int argc,char*argv[]){rclcpp::init(argc,argv);rclcpp::spin(std::make_shared<NavLiteNode>());rclcpp::shutdown();return 0;}
|
||||
Reference in New Issue
Block a user