Skip to content

Commit

Permalink
add omni_type option
Browse files Browse the repository at this point in the history
  • Loading branch information
@pingplug
pingplug committed Dec 24, 2018
1 parent c7bae48 commit 893d5ee
Show file tree
Hide file tree
Showing 8 changed files with 813 additions and 372 deletions.
306 changes: 155 additions & 151 deletions cfg/TebLocalPlannerReconfigure.cfg
View file

Large diffs are not rendered by default.

18 changes: 9 additions & 9 deletions include/teb_local_planner/g2o_types/edge_acceleration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1733,7 +1733,7 @@ class EdgeAccelerationHolonomic3 : public BaseTebMultiEdge<3, double>

// error
_error[0] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1-a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeAccelerationHolonomic3::computeError(): a1=%f\n",a1);
Expand Down Expand Up @@ -1825,7 +1825,7 @@ class EdgeAccelerationHolonomic3Start : public BaseTebMultiEdge<3, const geometr

// error
_error[0] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1-a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeAccelerationHolonomic3Start::computeError(): a1=%f\n",a1);
Expand Down Expand Up @@ -1927,7 +1927,7 @@ class EdgeAccelerationHolonomic3Goal : public BaseTebMultiEdge<3, const geometry

// error
_error[0] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1-a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeAccelerationHolonomic3Goal::computeError(): a1=%f\n",a1);
Expand Down Expand Up @@ -2039,8 +2039,8 @@ class EdgeAccelerationHolonomic4 : public BaseTebMultiEdge<3, double>

// error
_error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(-a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1*0.5-a2-a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeAccelerationHolonomic4::computeError(): a1=%f\n",a1);
ROS_ASSERT_MSG(std::isfinite(a2), "EdgeAccelerationHolonomic4::computeError(): a2=%f\n",a2);
Expand Down Expand Up @@ -2131,8 +2131,8 @@ class EdgeAccelerationHolonomic4Start : public BaseTebMultiEdge<3, const geometr

// error
_error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(-a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1*0.5-a2-a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeAccelerationHolonomic4Start::computeError(): a1=%f\n",a1);
ROS_ASSERT_MSG(std::isfinite(a2), "EdgeAccelerationHolonomic4Start::computeError(): a2=%f\n",a2);
Expand Down Expand Up @@ -2233,8 +2233,8 @@ class EdgeAccelerationHolonomic4Goal : public BaseTebMultiEdge<3, const geometry

// error
_error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(-a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1*0.5-a2-a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeAccelerationHolonomic4Goal::computeError(): a1=%f\n",a1);
ROS_ASSERT_MSG(std::isfinite(a2), "EdgeAccelerationHolonomic4Goal::computeError(): a2=%f\n",a2);
Expand Down
6 changes: 3 additions & 3 deletions include/teb_local_planner/g2o_types/edge_velocity.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -568,7 +568,7 @@ class EdgeVelocityHolonomic3 : public BaseTebMultiEdge<3, double>

// error
_error[0] = penaltyBoundToInterval(a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1+a2*0.5-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1-a2*0.5+a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeVelocityHolonomic3::computeError(): a1=%f\n",a1);
Expand Down Expand Up @@ -647,8 +647,8 @@ class EdgeVelocityHolonomic4 : public BaseTebMultiEdge<3, double>

// error
_error[0] = penaltyBoundToInterval(a1+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(-a1*0.5+a2+a3, 1.0, cfg_->optim.penalty_epsilon);
_error[1] = penaltyBoundToInterval(a1*0.5+a2-a3, 1.0, cfg_->optim.penalty_epsilon);
_error[2] = penaltyBoundToInterval(a1*0.5-a2-a3, 1.0, cfg_->optim.penalty_epsilon);

ROS_ASSERT_MSG(std::isfinite(a1), "EdgeVelocityHolonomic4::computeError(): a1=%f\n",a1);
ROS_ASSERT_MSG(std::isfinite(a2), "EdgeVelocityHolonomic4::computeError(): a2=%f\n",a2);
Expand Down
2 changes: 2 additions & 0 deletions include/teb_local_planner/teb_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,7 @@ class TebConfig
double acc_lim_theta; //!< Maximum angular acceleration of the robot
double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero);
double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!
int omni_type; //!< Type of the holonomic robot, possible choices are 0..4. Type 0: ideal; x:y should be 1:1. Type 1: 4 wheel, 45 deg; x:y should be 1:1. Type 2: 4 wheel, 0 deg; x:y should be 1:1. Type 3: 3 wheel, 0 deg or 180 deg; x:y should be 2:sqrt(3). Type 4: 3 wheel, 90 deg or 270 deg; x:y should be sqrt(3):2.
bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')
bool is_footprint_dynamic; //<! If true, updated the footprint before checking trajectory feasibility
} robot; //!< Robot related parameters
Expand Down Expand Up @@ -248,6 +249,7 @@ class TebConfig
robot.acc_lim_theta = 0.5;
robot.min_turning_radius = 0;
robot.wheelbase = 1.0;
robot.omni_type = 0;
robot.cmd_angle_instead_rotvel = false;
robot.is_footprint_dynamic = false;

Expand Down
16 changes: 16 additions & 0 deletions include/teb_local_planner/teb_local_planner_ros.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -315,6 +315,22 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner
double max_vel_theta, double max_vel_x_backwards) const;


/**
* @brief Saturate the translational and angular velocity to given limits for different holonomic type.
*
* The limit of the translational velocity for backwards driving can NOT be changed independently.
* @param[in,out] vx The translational velocity that should be saturated.
* @param[in,out] vy Strafing velocity which can be nonzero for holonomic robots
* @param[in,out] omega The angular velocity that should be saturated.
* @param max_vel_x Maximum translational velocity for forward driving
* @param max_vel_y Maximum strafing velocity (for holonomic robots)
* @param max_vel_theta Maximum (absolute) angular velocity
* @param omni_type Type of the holonomic robot, see teb_config.h for details
*/
void saturateVelocityHolonomic(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y,
double max_vel_theta, int omni_type) const;


/**
* @brief Convert translational and rotational velocities to a steering angle of a carlike robot
*
Expand Down
Loading

0 comments on commit 893d5ee

Please sign in to comment.