/** Setting up dynamic_reconfigure server for the ObstacleFeedConfig parameters **/ ros::NodeHandle nh_obstacle("lmpcc_obstacle_feed"); reconfigure_server_.reset(new dynamic_reconfigure::Server<lmpcc_obstacle_feed::ObstacleFeedConfig>(reconfig_mutex_, nh_obstacle)); reconfigure_server_->setCallback(boost::bind(&ObstacleFeed::reconfigureCallback,this,_1,_2));
重置障碍物的数目
1 2 3 4
// setting resize value obstacles_.lmpcc_obstacles.resize(lmpcc_obstacle_feed_config_->obstacle_number_); //resize according to number of obstacles obstacles_.header.stamp = ros::Time::now(); obstacles_.header.frame_id = lmpcc_obstacle_feed_config_->planning_frame_;
obstacles_.lmpcc_obstacles[obst_it].minor_semiaxis.resize(lmpcc_obstacle_feed_config_->discretization_steps_); //resize according to time horizon obstacles_.lmpcc_obstacles[obst_it].major_semiaxis.resize(lmpcc_obstacle_feed_config_->discretization_steps_);
// Check predefined obstacles for errors if (!(lmpcc_obstacle_feed_config_->obst_pose_x_.size() == lmpcc_obstacle_feed_config_->obst_pose_y_.size() && lmpcc_obstacle_feed_config_->obst_pose_x_.size() == lmpcc_obstacle_feed_config_->obst_pose_heading_.size() && lmpcc_obstacle_feed_config_->obst_pose_x_.size() == lmpcc_obstacle_feed_config_->obst_dim_minor_.size() && lmpcc_obstacle_feed_config_->obst_pose_x_.size() == lmpcc_obstacle_feed_config_->obst_dim_major_.size())) { ROS_ERROR("Predefined obstacle arrays are not of the same length!");
if (mag_q != 1) { //ROS_WARN("Quaternion magnitude not equal to one, making required modifications"); obst.pose.orientation.x = 0; obst.pose.orientation.y = 0; obst.pose.orientation.z = 0; obst.pose.orientation.w = 1; }
获得主轴和副轴的值
1 2 3 4
//getting the major and minor semi axis values obst.minor_semiaxis[0]= lmpcc_obstacle_feed_config_->obst_dim_major_[0]; obst.major_semiaxis[0]= lmpcc_obstacle_feed_config_->obst_dim_minor_[0]; obst.trajectory.poses[0].pose = obst.pose;
for(int i = 0; i< local_obstacles.lmpcc_obstacles.size(); i++) {}
首先进行坐标变换
❗❗❗base_link实际使用时需要更改
1 2
// transform the pose to base_link in order to calculate the distance to the obstacle transformPose(person.header.frame_id, "base_link", local_obstacles.lmpcc_obstacles[i].pose);
然后获取障碍物的坐标并计算距离原点的距离
1 2 3 4 5 6
//get obstacle coordinates in base_link frame Xp = local_obstacles.lmpcc_obstacles[i].pose.position.x; Yp = local_obstacles.lmpcc_obstacles[i].pose.position.y;
//distance between the Prius and the obstacle distance = sqrt(pow(Xp, 2) + pow(Yp, 2));
再将坐标系转换至planning_frame_
1 2
//transform the pose back to planning_frame for further calculations transformPose("base_link", lmpcc_obstacle_feed_config_->planning_frame_, local_obstacles.lmpcc_obstacles[i].pose);
获得障碍物的主轴和副轴信息
1 2 3 4 5 6 7
//get the bounding box pose measurements for the obstacle obst1.pose = local_obstacles.lmpcc_obstacles[i].pose; //get the major and minor semi axis readings of the stored obstacles for(int j=0; j< obst1.major_semiaxis.size(); j++){ obst1.major_semiaxis[j] = local_obstacles.lmpcc_obstacles[i].major_semiaxis[j]; obst1.minor_semiaxis[j] = local_obstacles.lmpcc_obstacles[i].minor_semiaxis[j]; }
获得障碍物的pose信息
1 2 3 4 5 6 7 8
//get the trajectory pose readings of the stored obstacles for(int k=0; k< lmpcc_obstacle_feed_config_->discretization_steps_; k++){ obst1.trajectory.poses[k].pose = local_obstacles.lmpcc_obstacles[i].trajectory.poses[k].pose; //obst1.trajectory.poses[k].pose.position.y = local_obstacles.lmpcc_obstacles[i].trajectory.poses[k].pose.position.y; //transform the pose back to planning_frame for further calculations if(person.header.frame_id!=lmpcc_obstacle_feed_config_->planning_frame_) transformPose(person.header.frame_id, lmpcc_obstacle_feed_config_->planning_frame_,obst1.trajectory.poses[k].pose); }
去除距离太远的障碍物
1 2 3 4 5
//filter out the obstacles that are farther away than the threshold distance value if (distance < distance_) { filter_obstacles.lmpcc_obstacles.push_back(obst1); objectDistances.push_back(distance); }
将经过选择后的障碍物的信息填充到椭圆中
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
//fit the ellipse to the obstacles for(int filt_obst_it = 0; filt_obst_it < filter_obstacles.lmpcc_obstacles.size(); filt_obst_it ++) { //fitting the ellipse ellipse = FitEllipse(filter_obstacles.lmpcc_obstacles[filt_obst_it], objectDistances[filt_obst_it]);
//estimating the ellipse trajectory over the defined horizon for (int traj_it = 0; traj_it < lmpcc_obstacle_feed_config_->discretization_steps_; traj_it++) { ellipse.trajectory.poses[traj_it].header.stamp = ros::Time::now(); ellipse.trajectory.poses[traj_it].header.frame_id = lmpcc_obstacle_feed_config_->planning_frame_; ellipse.trajectory.header.frame_id = lmpcc_obstacle_feed_config_->planning_frame_; ellipse.trajectory.poses[traj_it].pose = filter_obstacles.lmpcc_obstacles[filt_obst_it].trajectory.poses[traj_it].pose; }
//store the calculated ellipse ellipses.lmpcc_obstacles.push_back(ellipse); }
将椭圆按照距离由近到远的顺序进行排序
1 2
//order the stored ellipses with the closest one being first OrderObstacles(ellipses);
将椭圆信息保存在local_ellipses中
1 2 3 4 5 6 7
//find the minimum out of the number of detected obstacles and the default number of eligible obstacles to implement the algorithm int n = std::min(int(ellipses.lmpcc_obstacles.size()),N_obstacles_); //ROS_INFO_STREAM("Publish and visualize obstacles: " << n); // store the ordered ellipses in a vector local_ellipses for (int ellipses_it = 0; ellipses_it < n; ellipses_it++) { local_ellipses.lmpcc_obstacles.push_back(ellipses.lmpcc_obstacles[ellipses_it]); }
如果探测到的障碍物小于默认数值，将剩余的障碍进行随机初始化
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
//if the number of detected obstacle is less than the default number, randomly initialize the remaining number of required obstacles for (int ellipses_it = n; ellipses_it < N_obstacles_ ; ellipses_it++) { ellipse.pose.position.x = 1000; ellipse.pose.position.y = 1000; ellipse.minor_semiaxis.resize(lmpcc_obstacle_feed_config_->discretization_steps_); ellipse.major_semiaxis.resize(lmpcc_obstacle_feed_config_->discretization_steps_); for (int traj_it = 0; traj_it < lmpcc_obstacle_feed_config_->discretization_steps_; traj_it++) { ellipse.trajectory.poses[traj_it].header.stamp = ros::Time::now(); ellipse.trajectory.poses[traj_it].header.frame_id = lmpcc_obstacle_feed_config_->planning_frame_; ellipse.trajectory.header.frame_id = lmpcc_obstacle_feed_config_->planning_frame_; ellipse.trajectory.poses[traj_it].pose.position.x = 1000; ellipse.trajectory.poses[traj_it].pose.position.y = 1000; } local_ellipses.lmpcc_obstacles.push_back(ellipse); }
发布障碍物信息
1 2 3 4 5
//publish and visualize the detected obstacles if(local_ellipses.lmpcc_obstacles.size()>0){ publishObstacles(local_ellipses); visualizeObstacles(local_ellipses); }
if (!nh.getParam ("obstacle_feed_mode", obstacle_feed_mode_) ) { ROS_WARN(" Parameter 'obstacle_feed_mode_' not set on %s node " , ros::this_node::getName().c_str()); returnfalse; }
Obstacle_Prediction::Obstacle_Prediction(doubledelta_t, int horizon_N): delta_t_(delta_t), horizon_N_(horizon_N) { ROS_INFO("In class constructor of Obstacle_Prediction");
// Initialization the state covariance, this is important for starting the Kalman filter double cov_pos = 1^2; double cov_vel = 1^2; state_cov_estimated_.setZero(); state_cov_estimated_(0, 0) = cov_pos; state_cov_estimated_(1, 1) = cov_pos; state_cov_estimated_(2, 2) = cov_pos; state_cov_estimated_(3, 3) = cov_vel; state_cov_estimated_(4, 4) = cov_vel; state_cov_estimated_(5, 5) = cov_vel;
// Other initialization pos_measured_.setZero(); state_estimated_.setZero();