add footprint inflation for higher speeds #32
Conversation
mikeferguson
force-pushed
the
inflate_footprint
branch
from
e768962
to
3c866bb
Compare
mikeferguson
force-pushed
the
inflate_footprint
branch
from
e59619d
to
8a6562f
Compare
| } | ||
|
|
||
| ROS_ERROR("No pose in path was reachable"); | ||
| // Really shouldn't hit this |
There was a problem hiding this comment.
this is outside of a while(true) loop with no breaks... so it is impossible to get here
There was a problem hiding this comment.
yes, but interestingly the compiler still insists I need a return value here... so I left the messages/comments as well
| collision_points_->markers.resize(0); | ||
| } | ||
| // Get control and path, iteratively | ||
| while (true) |
There was a problem hiding this comment.
maybe add a timeout just in case we don't hit any of the return conditions.
| geometry_msgs::PoseStamped robot_velocity; | ||
| odom_helper_.getRobotVel(robot_velocity); | ||
| max_vel_x = robot_velocity.pose.position.x + (acc_lim_x_ * acc_dt_); | ||
| max_vel_x = std::min(max_vel_x, max_vel_x_); |
There was a problem hiding this comment.
this line needed a few takes for me to see the underscore difference
| if (!odom_helper_.getOdomTopic().empty()) | ||
| // Iteratively try to find a path, incrementally reducing the velocity | ||
| double sim_velocity = max_vel_x; | ||
| do |
| // Configure controller max velocity | ||
| controller_->setVelocityLimits(min_vel_x_, sim_velocity, max_vel_theta_); | ||
| // Actually simulate our path | ||
| if (simulate(target_pose, cmd_vel)) |
There was a problem hiding this comment.
wondering if simulate() should actually be get_cmd_vel()
| return true; | ||
| } | ||
| // Reduce velocity and try again for same target_pose | ||
| sim_velocity -= scaling_step_; |
There was a problem hiding this comment.
interesting, so we start at the highest velocity, and keep iterating until we are under scaling_vel_x_? Why are we using scaling_vel_x_ here instead of just min_vel_x?
There was a problem hiding this comment.
once you go below scaling_vel_x you are no longer shrinking the footprint (it's at scale = 1.0) - so if the path to the target_pose was blocked at the previous iteration, it will still be blocked at any lower velocity.
No description provided.