这两个文件规划速度控制, 核心的两个函数是如下两个: 一个是差分速度控制, 一个是全向速度控制
/**
* @brief calculates a twist feedforward command from the current band
* @param refernce to the twist cmd
*/
// 全向速度控制, 差分速度控制在里面作为一个 if 分支
bool getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_reached);
// 差分速度控制
bool getTwistDifferentialDrive(geometry_msgs::Twist& twist_cmd, bool& goal_reached);
eband_trajectory_controller.h
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Christian Connette
*********************************************************************/
#ifndef EBAND_TRAJECTORY_CONTROLLER_H_
#define EBAND_TRAJECTORY_CONTROLLER_H_
#include <ros/ros.h>
#include <ros/assert.h>
// classes which are part of this package
#include <eband_local_planner/conversions_and_types.h>
#include <eband_local_planner/eband_visualization.h>
#include <eband_local_planner/EBandPlannerConfig.h>
// geometry_msg
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
// nav_msgs
#include <nav_msgs/Odometry.h>
// geometry
#include <angles/angles.h>
// PID control library
#include <control_toolbox/pid.h>
namespace eband_local_planner{
/**
* @class EBandPlanner
* @brief Implements the Elastic Band Method for SE2-Manifold (mobile Base)
*/
class EBandTrajectoryCtrl{
public:
/**
* @brief Default constructor
*/
EBandTrajectoryCtrl();
/**
* @brief Constructs the elastic band object
* @param name The name to give this instance of the elastic band local planner
* @param costmap The cost map to use for assigning costs to trajectories
*/
EBandTrajectoryCtrl(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Destructor
*/
~EBandTrajectoryCtrl();
/**
* @brief Initializes the elastic band class by accesing costmap and loading parameters
* @param name The name to give this instance of the trajectory planner
* @param costmap The cost map to use for assigning costs to trajectories
*/
void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
/**
* @brief Reconfigures the parameters of the planner
* @param config The dynamic reconfigure configuration
*/
void reconfigure(EBandPlannerConfig& config);
/**
* @brief passes a reference to the eband visualization object which can be used to visualize the band optimization
* @param pointer to visualization object
*/
void setVisualization(boost::shared_ptr<EBandVisualization> target_visual);
/**
* @brief This sets the elastic_band to the trajectory controller
* @param reference via which the band is passed
*/
bool setBand(const std::vector<Bubble>& elastic_band);
/**
* @brief This sets the robot velocity as provided by odometry
* @param reference via which odometry is passed
*/
// 接收里程计, 读取速度信息
// eband_local_planner 会订阅 /odom 话题获取里程计
bool setOdometry(const nav_msgs::Odometry& odometry);
/**
* @brief calculates a twist feedforward command from the current band
* @param refernce to the twist cmd
*/
// 全向速度控制, 差分速度控制在里面作为一个 if 分支
bool getTwist(geometry_msgs::Twist& twist_cmd, bool& goal_reached);
// 差分速度控制
bool getTwistDifferentialDrive(geometry_msgs::Twist& twist_cmd, bool& goal_reached);
private:
// pointer to external objects (do NOT delete object)
costmap_2d::Costmap2DROS* costmap_ros_; ///<@brief pointer to costmap
boost::shared_ptr<EBandVisualization> target_visual_; // pointer to visualization object
control_toolbox::Pid pid_;
// parameters
bool differential_drive_hack_;
double k_p_, k_nu_, ctrl_freq_;
double acc_max_, virt_mass_;
double max_vel_lin_, max_vel_th_, min_vel_lin_, min_vel_th_;
double min_in_place_vel_th_;
double in_place_trans_vel_;
double tolerance_trans_, tolerance_rot_;
double acc_max_trans_, acc_max_rot_;
double rotation_correction_threshold_; // We'll do rotation correction if we're at least this far from the goal
// diff drive only parameters
double bubble_velocity_multiplier_;
double rotation_threshold_multiplier_;
bool disallow_hysteresis_;
bool in_final_goal_turn_;
// flags
bool initialized_, band_set_, visualization_;
// data
std::vector<Bubble> elastic_band_;
geometry_msgs::Twist odom_vel_;
geometry_msgs::Twist last_vel_;
geometry_msgs::Pose ref_frame_band_;
///@brief defines sign of a double
inline double sign(double n)
{
return n < 0.0 ? -1.0 : 1.0;
}
/**
* @brief Transforms Pose of frame 1 and 2 into reference frame and gets difference of frame 1 and 2
* @param refernce to pose of frame1
* @param reference to pose of frame2
* @param reference to pose of reference frame
* @return vector from frame1 to frame2 in coordinates of the reference frame
*/
// 这俩函数差不多, 把坐标系1的速度按参考系转到2, 一个用于全向轮, 一个用于差速轮,其实都差不多
geometry_msgs::Twist getFrame1ToFrame2InRefFrame(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame);
geometry_msgs::Twist getFrame1ToFrame2InRefFrameNew(const geometry_msgs::Pose& frame1, const geometry_msgs::Pose& frame2, const geometry_msgs::Pose& ref_frame);
/**
* @param Transforms twist into a given reference frame
* @param Twist that shall be transformed
* @param refernce to pose of frame1
* @param reference to pose of frame2
* @return transformed twist
*/
// 这个函数简单的计算位置差,并直接把差值按 1:1 赋值给速度返回,没有scale
geometry_msgs::Twist transformTwistFromFrame1ToFrame2(const geometry_msgs::Twist& curr_twist, const geometry_msgs::Pose& frame1,
const geometry_msgs::Pose& frame2);
/**
* @brief limits the twist to the allowed range
* @param reference to unconstrained twist
* @return twist in allowed range
*/
// 阈值控制函数
geometry_msgs::Twist limitTwist(const geometry_msgs::Twist& twist);
/**
* @brief gets the max velocity allowed within this bubble depending on size of the bubble and pose and size of the following bubble
* @param number of the bubble of interest within the band
* @param band in which the bubble is in
* @return absolute value of maximum allowed velocity within this bubble
*/
double getBubbleTargetVel(const int& target_bub_num, const std::vector<Bubble>& band, geometry_msgs::Twist& VelDir);
};
};
#endif
eband_trajectory_controller.cpp 主要讲最核心的 getTwist(400行) 和 getTwistDifferentialDrive(200行)
// 差速速度控制
bool EBandTrajectoryCtrl::getTwistDifferentialDrive(
geometry_msgs::Twist &twist_cmd, bool &goal_reached) {
goal_reached = false;
geometry_msgs::Twist robot_cmd, bubble_diff;
robot_cmd.linear.x = 0.0;
robot_cmd.angular.z = 0.0;
bool command_provided = false;
// check if plugin initialized
if (!initialized_) {
ROS_ERROR("Requesting feedforward command from not initialized planner. "
"Please call initialize() before using this planner");
return false;
}
// check there is a plan at all (minimum 1 frame in this case, as robot + goal
// = plan)
// 路径太少则退出, 说明差不多到终点了或者路径规划错误
if ((!band_set_) || (elastic_band_.size() < 2)) {
ROS_WARN("Requesting feedforward command from empty band.");
return false;
}
// Get the differences between the first 2 bubbles in the robot's frame
// 获取机器人当前位姿到规划路径上即将到达的下一个点的距离向量, 是个Twist
bubble_diff = getFrame1ToFrame2InRefFrameNew(elastic_band_.at(0).center.pose,
elastic_band_.at(1).center.pose,
elastic_band_.at(0).center.pose);
float distance_from_goal = -1.0f;
// Check 1 快到终点的情况
// We need to check if we are within the threshold of the final destination
if (!command_provided) {
int curr_target_bubble = 1;
while (curr_target_bubble < ((int)elastic_band_.size()) - 1) {
curr_target_bubble++;
bubble_diff = getFrame1ToFrame2InRefFrameNew(
elastic_band_.at(0).center.pose,
elastic_band_.at(curr_target_bubble).center.pose,
elastic_band_.at(0).center.pose);
}
// if you go past tolerance, then try to get closer again
if (!disallow_hysteresis_) {
if (fabs(bubble_diff.linear.x) > tolerance_trans_ ||
fabs(bubble_diff.linear.y) > tolerance_trans_) {
in_final_goal_turn_ = false;
}
}
// Get the differences between the first 2 bubbles in the robot's frame
int goal_bubble = (((int)elastic_band_.size()) - 1);
// 计算当前位姿与局部地图下最远的目标点(截断全局路径的最后一个元素)的距离
bubble_diff = getFrame1ToFrame2InRefFrameNew(
elastic_band_.at(0).center.pose,
elastic_band_.at(goal_bubble).center.pose,
elastic_band_.at(0).center.pose);
distance_from_goal = sqrtf(bubble_diff.linear.x * bubble_diff.linear.x +
bubble_diff.linear.y * bubble_diff.linear.y);
// Get closer to the goal than the tolerance requires before starting the
// final turn. The final turn may cause you to move slightly out of
// position
if ((fabs(bubble_diff.linear.x) <= 0.6 * tolerance_trans_ &&
fabs(bubble_diff.linear.y) <= 0.6 * tolerance_trans_) ||
in_final_goal_turn_) {
// Calculate orientation difference to goal orientation (not captured in
// bubble_diff)
double robot_yaw =
tf2::getYaw(elastic_band_.at(0).center.pose.orientation);
double goal_yaw =
tf2::getYaw(elastic_band_.at((int)elastic_band_.size() - 1)
.center.pose.orientation);
float orientation_diff = angles::normalize_angle(goal_yaw - robot_yaw);
// 如果当前点与目标点方向角大于设置的旋转允差,那么原地旋转直到角度小于允差为止
if (fabs(orientation_diff) > tolerance_rot_) {
in_final_goal_turn_ = true;
ROS_DEBUG("Performing in place rotation for goal (diff): %f",
orientation_diff);
double rotation_sign = -2 * (orientation_diff < 0) + 1;
robot_cmd.angular.z =
rotation_sign * min_in_place_vel_th_ + k_p_ * orientation_diff;
if (fabs(robot_cmd.angular.z) > max_vel_th_) { // limit max rotation
robot_cmd.angular.z = rotation_sign * max_vel_th_;
}
} else {
in_final_goal_turn_ = false; // Goal reached
// goal position reached
robot_cmd.linear.x = 0.0;
robot_cmd.angular.z = 0.0;
goal_reached = true;
}
command_provided = true;
}
}
// 2.未到终点的情况
// Get the differences between the first 2 bubbles in the robot's frame
bubble_diff = getFrame1ToFrame2InRefFrameNew(elastic_band_.at(0).center.pose,
elastic_band_.at(1).center.pose,
elastic_band_.at(0).center.pose);
// Check 2 - check if the robot's current pose is too misaligned with the next
// bubble
// 检查是否需要旋转
if (!command_provided) {
ROS_DEBUG(
"Goal has not been reached, performing checks to move towards goal");
// calculate an estimate of the in-place rotation threshold
// 到下一个泡泡的距离,即到规划路径上下一个点的距离
double distance_to_next_bubble =
sqrt(bubble_diff.linear.x * bubble_diff.linear.x +
bubble_diff.linear.y * bubble_diff.linear.y);
// 注意, 这里代码写死了 0.7 倍的泡泡半径, 有需要可以调调看看效果
// 这个 expansion 在 eband_local_planner 中由 costmap 计算的到障碍物的距离转化得来
double radius_of_next_bubble = 0.7 * elastic_band_.at(1).expansion;
// 这里定义了一个原地旋转阈值
// rotation_threshold_multiplier_ 作为参数在 yaml 文件中配置
// 把这个参数调大可以增大旋转阈值
// 这样机器人更倾向于边走边转, 而不是原地旋转到正确方向再走
// 调大这个参数就有一点儿全向移动那味儿了
double in_place_rotation_threshold =
rotation_threshold_multiplier_ *
fabs(atan2(radius_of_next_bubble, distance_to_next_bubble));
ROS_DEBUG("In-place rotation threshold: %f(%f,%f)",
in_place_rotation_threshold, radius_of_next_bubble,
distance_to_next_bubble);
// check if we are above this threshold, if so then perform in-place
// rotation
// 当前泡泡(位姿)到下一个泡泡的角度差大于原地旋转阈值, 则原地旋转
if (fabs(bubble_diff.angular.z) > in_place_rotation_threshold) {
robot_cmd.angular.z = k_p_ * bubble_diff.angular.z;
double rotation_sign = (bubble_diff.angular.z < 0) ? -1.0 : +1.0;
if (fabs(robot_cmd.angular.z) < min_in_place_vel_th_) {
robot_cmd.angular.z = rotation_sign * min_in_place_vel_th_;
}
if (fabs(robot_cmd.angular.z) > max_vel_th_) { // limit max rotation
robot_cmd.angular.z = rotation_sign * max_vel_th_;
}
ROS_DEBUG(
"Performing in place rotation for start (diff): %f with rot vel: %f",
bubble_diff.angular.z, robot_cmd.angular.z);
command_provided = true;
} else {
ROS_DEBUG("Don't need in-place rotation!");
}
}
// Check 3 - If we reach here, it means we need to use our PID controller to
// move towards the next bubble
// 以下是用 pid 计算速度的操作
if (!command_provided) {
// Select a linear velocity (based on the current bubble radius)
double forward_sign = -2 * (bubble_diff.linear.x < 0) + 1;
double bubble_radius = 0.7 * elastic_band_.at(0).expansion;
double velocity_multiplier = bubble_velocity_multiplier_ * bubble_radius;
double max_vel_lin = max_vel_lin_;
if (distance_from_goal < 0.75f) {
max_vel_lin = (max_vel_lin < 0.3) ? 0.15 : max_vel_lin / 2;
// 这里可以改成直接除2, 下面作者又会乘2, 所以不超过最大速度,
// 但最小速度可以小于 0.15
// max_vel_lin = max_vel_lin / 2;
}
// 差速控制比较简单
// 线速度由于只有x 方向, 直接置成最大线速度
double linear_velocity = velocity_multiplier * max_vel_lin;
linear_velocity *= cos(bubble_diff.angular.z); // decrease while turning
if (fabs(linear_velocity) > max_vel_lin_) {
linear_velocity = forward_sign * max_vel_lin_;
} else if (fabs(linear_velocity) < min_vel_lin_) {
linear_velocity = forward_sign * min_vel_lin_;
}
// Select an angular velocity (based on PID controller)
double error = bubble_diff.angular.z;
double rotation_sign = -2 * (bubble_diff.angular.z < 0) + 1;
// 角速度由位姿差给出
double angular_velocity = k_p_ * error;
if (fabs(angular_velocity) > max_vel_th_) {
angular_velocity = rotation_sign * max_vel_th_;
} else if (fabs(angular_velocity) < min_vel_th_) {
angular_velocity = rotation_sign * min_vel_th_;
}
ROS_DEBUG("Selected velocity: lin: %f, ang: %f", linear_velocity,
angular_velocity);
robot_cmd.linear.x = linear_velocity;
robot_cmd.angular.z = angular_velocity;
command_provided = true;
}
twist_cmd = robot_cmd;
ROS_DEBUG("Final command: %f, %f", twist_cmd.linear.x, twist_cmd.angular.z);
return true;
}
// 总的速度控制器
bool EBandTrajectoryCtrl::getTwist(geometry_msgs::Twist &twist_cmd,
bool &goal_reached) {
goal_reached = false;
if (differential_drive_hack_) {
// 调用差速控制器
return getTwistDifferentialDrive(twist_cmd, goal_reached);
}
// 以下则为全向速度控制
// init twist cmd to be handed back to caller
geometry_msgs::Twist robot_cmd, bubble_diff, control_deviation;
robot_cmd.linear.x = 0.0;
robot_cmd.linear.y = 0.0;
robot_cmd.linear.z = 0.0;
robot_cmd.angular.x = 0.0;
robot_cmd.angular.y = 0.0;
robot_cmd.angular.z = 0.0;
// make sure command vector is clean
twist_cmd = robot_cmd;
control_deviation = robot_cmd;
// check if plugin initialized
if (!initialized_) {
ROS_ERROR("Requesting feedforward command from not initialized planner. "
"Please call initialize() before using this planner");
return false;
}
// check there is a plan at all (minimum 1 frame in this case, as robot + goal
// = plan)
if ((!band_set_) || (elastic_band_.size() < 2)) {
ROS_WARN("Requesting feedforward command from empty band.");
return false;
}
// calc intersection of bubble-radius with sequence of vector connecting the
// bubbles
// get distance to target from bubble-expansion
double scaled_radius = 0.7 * elastic_band_.at(0).expansion;
// get difference and distance between bubbles in odometry frame
double bubble_distance, ang_pseudo_dist;
bubble_diff = getFrame1ToFrame2InRefFrame(elastic_band_.at(0).center.pose,
elastic_band_.at(1).center.pose,
ref_frame_band_);
// 这里会计算一个伪距离, diff_z*机器人外接圆半径(乘积是一段弧长)
ang_pseudo_dist =
bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
bubble_distance = sqrt((bubble_diff.linear.x * bubble_diff.linear.x) +
(bubble_diff.linear.y * bubble_diff.linear.y) +
(ang_pseudo_dist * ang_pseudo_dist));
if (visualization_) {
target_visual_->publishBubble("ctrl_target", 1, target_visual_->blue,
elastic_band_.at(0));
target_visual_->publishBubble("ctrl_target", 2, target_visual_->blue,
elastic_band_.at(1));
}
// by default our control deviation is the difference between the bubble
// centers
double abs_ctrl_dev;
control_deviation = bubble_diff;
// diff_z*机器人外接圆半径(一段弧长)
ang_pseudo_dist =
control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_);
// 点到点距离+diff_z*机器人外接圆半径(一段弧长)
abs_ctrl_dev =
sqrt((control_deviation.linear.x * control_deviation.linear.x) +
(control_deviation.linear.y * control_deviation.linear.y) +
(ang_pseudo_dist * ang_pseudo_dist));
// yet depending on the expansion of our bubble we might want to adapt this
// point
// 如果放缩的泡泡半径 < 到下一个泡泡的距离
// 说明相邻两个泡泡间隔较大,重叠部分较小
if (scaled_radius < bubble_distance) {
// ROS_INFO("scaled_radius < bubble_distance");
// triviale case - simply scale bubble_diff
// scale_difference < 1
double scale_difference = scaled_radius / bubble_distance;
// 两个泡泡间隔较大,重叠部分较小,说明路径两个点间距离较大
// 故减小速度, 以应对突发情况
bubble_diff.linear.x *= scale_difference;
bubble_diff.linear.y *= scale_difference;
bubble_diff.angular.z *= scale_difference;
// set controls
// control_deviation 这个很关键, 参与速度cmd计算
control_deviation = bubble_diff;
}
// if scaled_radius = bubble_distance -- we have nothing to do at all
// 如果放缩的泡泡半径 > 到下一个泡泡的距离
// 说明相邻两个泡泡间隔较小,重叠部分较大
if (scaled_radius > bubble_distance) {
// ROS_INFO("scaled_radius > bubble_distance, check next bubble");
// o.k. now we have to do a little bit more -> check next but one bubble
if (elastic_band_.size() > 2) {
// get difference between next and next but one bubble
double next_bubble_distance;
// 计算当前点往后走两个泡泡的距离
geometry_msgs::Twist next_bubble_diff;
next_bubble_diff = getFrame1ToFrame2InRefFrame(
elastic_band_.at(1).center.pose, elastic_band_.at(2).center.pose,
ref_frame_band_);
ang_pseudo_dist =
next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
next_bubble_distance =
sqrt((next_bubble_diff.linear.x * next_bubble_diff.linear.x) +
(next_bubble_diff.linear.y * next_bubble_diff.linear.y) +
(ang_pseudo_dist * ang_pseudo_dist));
// 如果放缩的泡泡半径 > 到下两个泡泡的距离
// 说明相邻两个泡泡间隔较小,重叠部分很大
// 即一个单位的放缩的泡泡半径可以覆盖到两个泡泡,泡泡间隔非常紧
if (scaled_radius > (bubble_distance + next_bubble_distance)) {
// ROS_INFO("scaled_radius > bubble_distance + next_bubble_distance");
// we should normally not end up here - but just to be sure
// 适当增大速度,要不然太小了
control_deviation.linear.x =
bubble_diff.linear.x + next_bubble_diff.linear.x;
control_deviation.linear.y =
bubble_diff.linear.y + next_bubble_diff.linear.y;
control_deviation.angular.z =
bubble_diff.angular.z + next_bubble_diff.angular.z;
// done
if (visualization_)
target_visual_->publishBubble("ctrl_target", 3, target_visual_->red,
elastic_band_.at(2));
} else {
// ROS_INFO("scaled_radius < bubble_distance + next_bubble_distance");
// 如果 到下一个泡泡的距离 < 放缩的泡泡半径 < 到下两个泡泡的距离
if (visualization_) {
target_visual_->publishBubble("ctrl_target", 3, target_visual_->red,
elastic_band_.at(2));
}
// we want to calculate intersection point of bubble ...
// ... and vector connecting the following bubbles
double b_distance, cosine_at_bub;
double vec_prod, norm_vec1, norm_vec2;
double ang_pseudo_dist1, ang_pseudo_dist2;
// get distance between next bubble center and intersection point
ang_pseudo_dist1 =
bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
ang_pseudo_dist2 =
next_bubble_diff.angular.z * getCircumscribedRadius(*costmap_ros_);
// careful! - we need this sign because of the direction of the vectors
// and the definition of the vector-product
// 注意这里有个负号
vec_prod = -((bubble_diff.linear.x * next_bubble_diff.linear.x) +
(bubble_diff.linear.y * next_bubble_diff.linear.y) +
(ang_pseudo_dist1 * ang_pseudo_dist2));
// 到第一个泡泡中心的距离(加了一个伪弧长)
norm_vec1 = sqrt((bubble_diff.linear.x * bubble_diff.linear.x) +
(bubble_diff.linear.y * bubble_diff.linear.y) +
(ang_pseudo_dist1 * ang_pseudo_dist1));
// 第一个泡泡中心到第二个泡泡中心的距离(加了一个伪弧长)
norm_vec2 =
sqrt((next_bubble_diff.linear.x * next_bubble_diff.linear.x) +
(next_bubble_diff.linear.y * next_bubble_diff.linear.y) +
(ang_pseudo_dist2 * ang_pseudo_dist2));
// reform the cosine-rule
// 这里用的某个规则重置 cos
cosine_at_bub = vec_prod / norm_vec1 / norm_vec2;
b_distance = bubble_distance * cosine_at_bub +
sqrt(scaled_radius * scaled_radius -
bubble_distance * bubble_distance *
(1.0 - cosine_at_bub * cosine_at_bub));
// get difference vector from next_bubble to intersection point
// 这里计算了一个放缩因子
double scale_next_difference = b_distance / next_bubble_distance;
next_bubble_diff.linear.x *= scale_next_difference;
next_bubble_diff.linear.y *= scale_next_difference;
next_bubble_diff.angular.z *= scale_next_difference;
// and finally get the control deviation
control_deviation.linear.x =
bubble_diff.linear.x + next_bubble_diff.linear.x;
control_deviation.linear.y =
bubble_diff.linear.y + next_bubble_diff.linear.y;
control_deviation.angular.z =
bubble_diff.angular.z + next_bubble_diff.angular.z;
// done
}
}
}
// plot control deviation
ang_pseudo_dist =
control_deviation.angular.z * getCircumscribedRadius(*costmap_ros_);
abs_ctrl_dev =
sqrt((control_deviation.linear.x * control_deviation.linear.x) +
(control_deviation.linear.y * control_deviation.linear.y) +
(ang_pseudo_dist * ang_pseudo_dist));
if (visualization_) {
// compose bubble from ctrl-target
geometry_msgs::Pose2D tmp_bubble_2d, curr_bubble_2d;
geometry_msgs::Pose tmp_pose;
// init bubble for visualization
Bubble new_bubble = elastic_band_.at(0);
PoseToPose2D(elastic_band_.at(0).center.pose, curr_bubble_2d);
tmp_bubble_2d.x = curr_bubble_2d.x + control_deviation.linear.x;
tmp_bubble_2d.y = curr_bubble_2d.y + control_deviation.linear.y;
tmp_bubble_2d.theta = curr_bubble_2d.theta + control_deviation.angular.z;
Pose2DToPose(tmp_pose, tmp_bubble_2d);
new_bubble.center.pose = tmp_pose;
new_bubble.expansion = 0.1; // just draw a small bubble
target_visual_->publishBubble("ctrl_target", 0, target_visual_->red,
new_bubble);
}
// 这里计算当前位姿到终点还有多远
const geometry_msgs::Point &goal =
(--elastic_band_.end())->center.pose.position;
const double dx = elastic_band_.at(0).center.pose.position.x - goal.x;
const double dy = elastic_band_.at(0).center.pose.position.y - goal.y;
const double dist_to_goal = sqrt(dx * dx + dy * dy);
// Assuming we're far enough from the final goal, make sure to rotate so
// we're facing the right way
// 如果到最终目标点距离大于阈值,出发前要校正方向
// 即使是全向轮,也要校正方向
// 调大这个阈值, 只要在阈值范围内, 规划出来的路径可以全向移动!
if (dist_to_goal > rotation_correction_threshold_) {
const double angular_diff =
angularDiff(control_deviation, elastic_band_.at(0).center.pose);
const double vel =
pid_.computeCommand(angular_diff, ros::Duration(1 / ctrl_freq_));
const double mult = fabs(vel) > max_vel_th_ ? max_vel_th_ / fabs(vel) : 1.0;
control_deviation.angular.z = vel * mult;
const double abs_vel = fabs(control_deviation.angular.z);
ROS_DEBUG_THROTTLE_NAMED(1.0, "angle_correction",
"Angular diff is %.2f and desired angular "
"vel is %.2f. Initial translation velocity "
"is %.2f, %.2f",
angular_diff, control_deviation.angular.z,
control_deviation.linear.x,
control_deviation.linear.y);
const double trans_mult = max(
0.01,
1.0 -
abs_vel /
max_vel_th_); // There are some weird tf errors if I let it be 0
control_deviation.linear.x *= trans_mult;
control_deviation.linear.y *= trans_mult;
ROS_DEBUG_THROTTLE_NAMED(1.0, "angle_correction",
"Translation multiplier is %.2f and scaled "
"translational velocity is %.2f, %.2f",
trans_mult, control_deviation.linear.x,
control_deviation.linear.y);
} else
ROS_DEBUG_THROTTLE_NAMED(1.0, "angle_correction",
"Not applying angle correction because "
"distance to goal is %.2f",
dist_to_goal);
// 以下主要是通过 Pid 控制速度的 scale 以及连续性
// now the actual control procedure start (using attractive Potentials)
geometry_msgs::Twist desired_velocity, currbub_maxvel_dir;
double desvel_abs, desvel_abs_trans, currbub_maxvel_abs;
double scale_des_vel;
desired_velocity = robot_cmd;
currbub_maxvel_dir = robot_cmd;
// calculate "equilibrium velocity" (Khatib86 - Realtime Obstacle Avoidance)
// k_p_, k_nu_ 分别是参数文件里的 k_prop 和 k_damp
desired_velocity.linear.x = k_p_ / k_nu_ * control_deviation.linear.x;
desired_velocity.linear.y = k_p_ / k_nu_ * control_deviation.linear.y;
desired_velocity.angular.z = k_p_ / k_nu_ * control_deviation.angular.z;
// robot_cmd = desired_velocity;
// get max vel for current bubble
int curr_bub_num = 0;
currbub_maxvel_abs =
getBubbleTargetVel(curr_bub_num, elastic_band_, currbub_maxvel_dir);
// if neccessarry scale desired vel to stay lower than currbub_maxvel_abs
ang_pseudo_dist =
desired_velocity.angular.z * getCircumscribedRadius(*costmap_ros_);
desvel_abs = sqrt((desired_velocity.linear.x * desired_velocity.linear.x) +
(desired_velocity.linear.y * desired_velocity.linear.y) +
(ang_pseudo_dist * ang_pseudo_dist));
if (desvel_abs > currbub_maxvel_abs) {
scale_des_vel = currbub_maxvel_abs / desvel_abs;
desired_velocity.linear.x *= scale_des_vel;
desired_velocity.linear.y *= scale_des_vel;
desired_velocity.angular.z *= scale_des_vel;
}
// make sure to stay within velocity bounds for the robot
desvel_abs_trans =
sqrt((desired_velocity.linear.x * desired_velocity.linear.x) +
(desired_velocity.linear.y * desired_velocity.linear.y));
// for translation
if (desvel_abs_trans > max_vel_lin_) {
scale_des_vel = max_vel_lin_ / desvel_abs_trans;
desired_velocity.linear.x *= scale_des_vel;
desired_velocity.linear.y *= scale_des_vel;
// to make sure we are staying inside the bubble also scale rotation
desired_velocity.angular.z *= scale_des_vel;
}
// for rotation
if (fabs(desired_velocity.angular.z) > max_vel_th_) {
scale_des_vel = max_vel_th_ / fabs(desired_velocity.angular.z);
desired_velocity.angular.z *= scale_des_vel;
// to make sure we are staying inside the bubble also scale translation
desired_velocity.linear.x *= scale_des_vel;
desired_velocity.linear.y *= scale_des_vel;
}
// calculate resulting force (accel. resp.) (Khatib86 - Realtime Obstacle
// Avoidance)
geometry_msgs::Twist acc_desired;
acc_desired = robot_cmd;
acc_desired.linear.x = (1.0 / virt_mass_) * k_nu_ *
(desired_velocity.linear.x - last_vel_.linear.x);
acc_desired.linear.y = (1.0 / virt_mass_) * k_nu_ *
(desired_velocity.linear.y - last_vel_.linear.y);
acc_desired.angular.z = (1.0 / virt_mass_) * k_nu_ *
(desired_velocity.angular.z - last_vel_.angular.z);
// constrain acceleration
double scale_acc;
double abs_acc_trans = sqrt((acc_desired.linear.x * acc_desired.linear.x) +
(acc_desired.linear.y * acc_desired.linear.y));
if (abs_acc_trans > acc_max_trans_) {
scale_acc = acc_max_trans_ / abs_acc_trans;
acc_desired.linear.x *= scale_acc;
acc_desired.linear.y *= scale_acc;
// again - keep relations - stay in bubble
acc_desired.angular.z *= scale_acc;
}
if (fabs(acc_desired.angular.z) > acc_max_rot_) {
scale_acc = fabs(acc_desired.angular.z) / acc_max_rot_;
acc_desired.angular.z *= scale_acc;
// again - keep relations - stay in bubble
acc_desired.linear.x *= scale_acc;
acc_desired.linear.y *= scale_acc;
}
// and get velocity-cmds by integrating them over one time-step
last_vel_.linear.x = last_vel_.linear.x + acc_desired.linear.x / ctrl_freq_;
last_vel_.linear.y = last_vel_.linear.y + acc_desired.linear.y / ctrl_freq_;
last_vel_.angular.z =
last_vel_.angular.z + acc_desired.angular.z / ctrl_freq_;
// we are almost done now take into accoun stick-slip and similar nasty things
// last checks - limit current twist cmd (upper and lower bounds)
last_vel_ = limitTwist(last_vel_);
// finally set robot_cmd (to non-zero value)
robot_cmd = last_vel_;
// now convert into robot-body frame
robot_cmd = transformTwistFromFrame1ToFrame2(robot_cmd, ref_frame_band_,
elastic_band_.at(0).center.pose);
// check whether we reached the end of the band
// 快到终点了
int curr_target_bubble = 1;
while (fabs(bubble_diff.linear.x) <= tolerance_trans_ &&
fabs(bubble_diff.linear.y) <= tolerance_trans_ &&
fabs(bubble_diff.angular.z) <= tolerance_rot_) {
if (curr_target_bubble < ((int)elastic_band_.size()) - 1) {
curr_target_bubble++;
// transform next target bubble into robot-body frame
// and get difference to robot bubble
bubble_diff = getFrame1ToFrame2InRefFrame(
elastic_band_.at(0).center.pose,
elastic_band_.at(curr_target_bubble).center.pose, ref_frame_band_);
} else {
ROS_DEBUG_THROTTLE_NAMED(1.0, "controller_state",
"Goal reached with distance %.2f, %.2f, %.2f"
"; sending zero velocity",
bubble_diff.linear.x, bubble_diff.linear.y,
bubble_diff.angular.z);
// goal position reached
robot_cmd.linear.x = 0.0;
robot_cmd.linear.y = 0.0;
robot_cmd.angular.z = 0.0;
// reset velocity
last_vel_.linear.x = 0.0;
last_vel_.linear.y = 0.0;
last_vel_.angular.z = 0.0;
goal_reached = true;
break;
}
}
twist_cmd = robot_cmd;
return true;
}
扫一扫
1691
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
