diff --git a/cfg/AckermannLocalPlanner.cfg b/cfg/AckermannLocalPlanner.cfg index 0748ebc70179f2978069afcc7cb16f95a354b565..a22f7c557e360847c22b3af2c1d1ee5835b0728e 100755 --- a/cfg/AckermannLocalPlanner.cfg +++ b/cfg/AckermannLocalPlanner.cfg @@ -40,55 +40,61 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -# Name Type Reconfiguration level Description Default Min Max -gen.add("max_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20) -gen.add("min_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10) -gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0) -gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0) - -gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0) -gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0) -gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0) - -gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0) -gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0) -gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0) - -gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325) - -gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0) -gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0) - -gen.add("trans_vel_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1) -gen.add("steer_angle_samples", int_t, 0, "The number of samples to use when exploring the y velocity space", 10, 1) - -gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False) -gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1) -gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1) -gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1) -gen.add("rot_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1) - -gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False) - -gen.add("max_trans_vel", double_t, 0, "maximum translational speed", 0.3, 0, 20) -gen.add("min_trans_vel", double_t, 0, "minimum translational speed", -0.3, -20, 0) -gen.add("max_trans_acc", double_t, 0, "maximum translational acceleration", 1.0, 0, 20) -gen.add("max_steer_angle", double_t, 0, "maximum steer angle", 0.45, 0, 0.54) -gen.add("min_steer_angle", double_t, 0, "minimum steer angle", -0.45, -0.54, 0) -gen.add("max_steer_vel", double_t, 0, "maximum steer speed", 1.0, 0, 2) -gen.add("min_steer_vel", double_t, 0, "minimum steer speed", -1.0, -2, 0) -gen.add("max_steer_acc", double_t, 0, "maximum steer acceleration", 0.36, 0, 5) -gen.add("axis_distance", double_t, 0, "distance between axes", 1.65, -2.0, 2) -gen.add("wheel_distance", double_t, 0, "distance between wheels", 1.2, 0, 2) -gen.add("wheel_radius", double_t, 0, "Wheel diameter", 0.436, 0, 2) -gen.add("split_ignore_length", double_t, 0, "Paths segments under this length will be ignored",1.0,0.0,2.0) -gen.add("cmd_vel_avg", int_t, 0, "Number of cmd_vel to average", 1, 1, 20) -gen.add("odom_avg", int_t, 0, "Number of odom to average", 1, 1, 20) - -gen.add("planner_patience", int_t, 0, "number of impossible paths before replanning",2, 0, 10) - -gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0 ,0, 500) -gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64) +# Name Type Reconfiguration level Description Default Min Max +gen.add("max_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 10, 0, 20) +gen.add("min_sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10) +gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0) +gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0) + +gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0) +gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0) +gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0) + +gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0) +gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0) +gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0) + +gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325) + +gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0) +gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0) + +gen.add("trans_vel_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1) +gen.add("steer_angle_samples", int_t, 0, "The number of samples to use when exploring the steer angle space", 10, 1) +gen.add("angular_vel_samples", int_t, 0, "The number of samples to use when exploring the angular velocity space", 10, 1) +gen.add("use_steer_angle_cmd", bool_t, 0, "Whether to use the steer angle or the angular velocity.", False) + +gen.add("prune_plan", bool_t, 0, "Start following closest point of global plan, not first point (if different).", False) +gen.add("xy_goal_tolerance", double_t, 0, "Within what maximum distance we consider the robot to be in goal", 0.1) +gen.add("yaw_goal_tolerance", double_t, 0, "Within what maximum angle difference we consider the robot to face goal direction", 0.1) +gen.add("trans_stopped_vel", double_t, 0, "Below what maximum velocity we consider the robot to be stopped in translation", 0.1) +gen.add("rot_stopped_vel", double_t, 0, "Below what maximum rotation velocity we consider the robot to be stopped in rotation", 0.1) + +gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False) + +gen.add("max_trans_vel", double_t, 0, "maximum translational speed", 0.3, 0, 20) +gen.add("min_trans_vel", double_t, 0, "minimum translational speed", -0.3, -20, 0) +gen.add("max_trans_acc", double_t, 0, "maximum translational acceleration", 1.0, 0, 20) +gen.add("max_steer_angle", double_t, 0, "maximum steer angle", 0.45, 0, 0.54) +gen.add("min_steer_angle", double_t, 0, "minimum steer angle", -0.45, -0.54, 0) +gen.add("max_steer_vel", double_t, 0, "maximum steer speed", 1.0, 0, 2) +gen.add("min_steer_vel", double_t, 0, "minimum steer speed", -1.0, -2, 0) +gen.add("max_steer_acc", double_t, 0, "maximum steer acceleration", 0.36, 0, 5) +gen.add("max_angular_vel", double_t, 0, "maximum angular speed", 0.3, 0, 20) +gen.add("min_angular_vel", double_t, 0, "minimum angular speed", -0.3, -20, 0) +gen.add("axis_distance", double_t, 0, "distance between axes", 1.65, -2.0, 2) +gen.add("wheel_distance", double_t, 0, "distance between wheels", 1.2, 0, 2) +gen.add("wheel_radius", double_t, 0, "Wheel diameter", 0.436, 0, 2) +gen.add("use_trans_vel_deadzone", bool_t, 0, "Whether to use a deadzone in the translational velocity or not", False) +gen.add("trans_vel_deadzone", double_t, 0, "Translatinal velocity minimum value", 0.1, 0.01, 1.0) +gen.add("split_ignore_length", double_t, 0, "Paths segments under this length will be ignored", 1.0, 0.0, 2.0) +gen.add("cmd_vel_avg", int_t, 0, "Number of cmd_vel to average", 1, 1, 20) +gen.add("odom_avg", int_t, 0, "Number of odom to average", 1, 1, 20) + +gen.add("planner_patience", int_t, 0, "number of impossible paths before replanning", 2, 0, 10) + +gen.add("hdiff_scale", double_t, 0, "The weight for the heading distance part of the cost function", 1.0, 0, 500) +gen.add("heading_points", int_t, 0, "The number of points to check the heading", 8, 1, 64) exit(gen.generate(PACKAGE, "AckermannLocalPlannerAlgorithm", "AckermannLocalPlanner")) diff --git a/include/ackermann_planner_limits.h b/include/ackermann_planner_limits.h index 1c3f9ff0518df893856bca5cb670fe7e4f784343..2907939a4ae5f3fe4728b7722964a64e0f7fe094 100644 --- a/include/ackermann_planner_limits.h +++ b/include/ackermann_planner_limits.h @@ -51,11 +51,16 @@ class AckermannPlannerLimits double max_steer_vel; double min_steer_vel; double max_steer_acc; + // angular velocity limits + double max_angular_vel; + double min_angular_vel; // kinematic attributes double axis_distance; double wheel_distance; double wheel_radius; // general planner limits + bool use_trans_vel_deadzone; + double trans_vel_deadzone; double split_ignore_length; bool prune_plan; double xy_goal_tolerance; @@ -77,11 +82,16 @@ class AckermannPlannerLimits double max_steer_vel, double min_steer_vel, double max_steer_acc, + // angular velocity limits + double max_angular_vel, + double min_angular_vel, // kinematic attributes double axis_distance, double wheel_distance, double wheel_radius, // general planner limits + bool use_trans_vel_deadzone, + double trans_vel_deadzone, double split_ignore_length, double xy_goal_tolerance, double yaw_goal_tolerance, @@ -96,9 +106,13 @@ class AckermannPlannerLimits max_steer_vel(max_steer_vel), min_steer_vel(min_steer_vel), max_steer_acc(max_steer_acc), + max_angular_vel(max_angular_vel), + min_angular_vel(min_angular_vel), axis_distance(axis_distance), wheel_distance(wheel_distance), wheel_radius(wheel_radius), + use_trans_vel_deadzone(use_trans_vel_deadzone), + trans_vel_deadzone(trans_vel_deadzone), split_ignore_length(split_ignore_length), prune_plan(prune_plan), xy_goal_tolerance(xy_goal_tolerance), diff --git a/include/ackermann_planner_ros.h b/include/ackermann_planner_ros.h index 33f48c061fb25ef8387c81cd53d00cb06b8b4f41..5b45993a7f58b41d6fe62a4b52fc97c86f2060cb 100644 --- a/include/ackermann_planner_ros.h +++ b/include/ackermann_planner_ros.h @@ -147,5 +147,6 @@ class AckermannPlannerROS : public nav_core::BaseLocalPlanner bool stucked; bool first; + bool use_steer_angle_cmd; }; #endif diff --git a/include/ackermann_trajectory_generator.h b/include/ackermann_trajectory_generator.h index b960cf55b3e079eaf5041ec14902d26bba8224fd..26e6a3f6a883c7f9a8ec7ae1e54922c857abf60e 100644 --- a/include/ackermann_trajectory_generator.h +++ b/include/ackermann_trajectory_generator.h @@ -91,6 +91,7 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG double min_sim_time, double sim_granularity, double angular_sim_granularity, + bool use_steer_angle_cmd, double sim_period = 0.0); /** @@ -126,6 +127,7 @@ class AckermannTrajectoryGenerator: public base_local_planner::TrajectorySampleG double max_sim_time_,min_sim_time_,sim_time_; double sim_granularity_, angular_sim_granularity_; double sim_period_; // only for dwa + bool use_steer_angle_cmd_; }; #endif /* SIMPLE_TRAJECTORY_GENERATOR_H_ */ diff --git a/params/ackermann_planner_params.yaml b/params/ackermann_planner_params.yaml index f8771137cd6b3131d109d6c80cf9c9e148144efb..ab581039c0fc6c37279ea2ec352f548efc61324f 100644 --- a/params/ackermann_planner_params.yaml +++ b/params/ackermann_planner_params.yaml @@ -1,50 +1,54 @@ +base_local_planner: "AckermannPlannerROS" + AckermannPlannerROS: - max_sim_time: 15 #8.0 + max_sim_time: 3.0 min_sim_time: 1.0 sim_granularity: 0.2 angular_sim_granularity: 0.02 - path_distance_bias: 3.0 - goal_distance_bias: 0.5 - occdist_scale: 0.01 + path_distance_bias: 50.0 + goal_distance_bias: 10.0 + occdist_scale: 0.05 -# stop_time_buffer: oscillation_reset_dist: 0.002 oscillation_reset_angle: 0.001 -# forward_point_distance: - -# scaling_speed: -# max_scaling_factor: - - trans_vel_samples: 21 - steer_angle_samples: 11 + trans_vel_samples: 31 + steer_angle_samples: 21 + angular_vel_samples: 21 + use_steer_angle_cmd: false prune_plan: true - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.5 + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.2 trans_stopped_vel: 0.1 rot_stopped_vel: 0.1 restore_defaults: false - max_trans_vel: 2.0 #1.0 - min_trans_vel: 0.0 #-1.0 - max_trans_acc: 0.3 - max_steer_angle: 0.35 - min_steer_angle: -0.35 - max_steer_vel: 0.5 - min_steer_vel: -0.5 - max_steer_acc: 1.0 - axis_distance: 4.3 - wheel_distance: 4.5 - wheel_radius: 0.65465 - - cmd_vel_avg: 10 + max_trans_vel: 1.0 + min_trans_vel: -1.0 + max_trans_acc: 0.5 + max_steer_angle: 0.5 + min_steer_angle: -0.5 + max_steer_vel: 1.0 + min_steer_vel: -1.0 + max_steer_acc: 5.0 + max_angular_vel: 0.5 + min_angular_vel: -0.5 + axis_distance: 0.36 + wheel_distance: 0.265 + wheel_radius: 0.05 + + use_trans_vel_deadzone: true + trans_vel_deadzone: 0.1 + + split_ignore_length: 0.1 + + cmd_vel_avg: 1 odom_avg: 1 planner_patience: 5 hdiff_scale: 0.5 heading_points: 8 - diff --git a/src/ackermann_planner.cpp b/src/ackermann_planner.cpp index 969dcd2b6323032e21f145e6e96457dc687dba4b..c62eaa373eb09caa2857419a604632d7285f9fd8 100644 --- a/src/ackermann_planner.cpp +++ b/src/ackermann_planner.cpp @@ -57,6 +57,7 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl config.min_sim_time, config.sim_granularity, config.angular_sim_granularity, + config.use_steer_angle_cmd, sim_period_); double resolution = planner_util_->get_costmap()->getResolution(); @@ -80,9 +81,12 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl // obstacle costs can vary due to scaling footprint feature obstacle_costs_.setParams(config.max_trans_vel, config.max_scaling_factor, config.scaling_speed); - int trans_vel_samp, steer_angle_samp; + int trans_vel_samp, steer_angle_angular_vel_samp; trans_vel_samp = config.trans_vel_samples; - steer_angle_samp = config.steer_angle_samples; + if(config.use_steer_angle_cmd) + steer_angle_angular_vel_samp = config.steer_angle_samples; + else + steer_angle_angular_vel_samp = config.angular_vel_samples; if (trans_vel_samp <= 0) { @@ -91,15 +95,18 @@ void AckermannPlanner::reconfigure(iri_ackermann_local_planner::AckermannLocalPl config.trans_vel_samples = trans_vel_samp; } - if (steer_angle_samp <= 0) + if (steer_angle_angular_vel_samp <= 0) { - ROS_WARN("You've specified that you don't want any samples in the steering dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead"); - steer_angle_samp = 1; - config.steer_angle_samples = steer_angle_samp; + ROS_WARN("You've specified that you don't want any samples in the steering/angular velocity dimension. We'll at least assume that you want to sample one value... so we're going to set vy_samples to 1 instead"); + steer_angle_angular_vel_samp = 1; + if(config.use_steer_angle_cmd) + config.steer_angle_samples = steer_angle_angular_vel_samp; + else + config.angular_vel_samples = steer_angle_angular_vel_samp; } vsamples_[0] = trans_vel_samp; - vsamples_[1] = steer_angle_samp; + vsamples_[1] = steer_angle_angular_vel_samp; } AckermannPlanner::AckermannPlanner(std::string name,AckermannPlannerUtil *planner_util) : diff --git a/src/ackermann_planner_ros.cpp b/src/ackermann_planner_ros.cpp index ca3bc68cdfb09fc5f6956a4026702d92e46e97ae..7d08e2f61c3ca02efffc82b9611667de89b91464 100644 --- a/src/ackermann_planner_ros.cpp +++ b/src/ackermann_planner_ros.cpp @@ -82,11 +82,15 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke limits.max_steer_vel = config.max_steer_vel; limits.min_steer_vel = config.min_steer_vel; limits.max_steer_acc = config.max_steer_acc; + limits.max_angular_vel = config.max_angular_vel; + limits.min_angular_vel = config.min_angular_vel; // kinematic attributes limits.axis_distance = config.axis_distance; limits.wheel_distance = config.wheel_distance; limits.wheel_radius = config.wheel_radius; // general planner limits + limits.use_trans_vel_deadzone = config.use_trans_vel_deadzone; + limits.trans_vel_deadzone = config.trans_vel_deadzone; limits.split_ignore_length = config.split_ignore_length; limits.xy_goal_tolerance = config.xy_goal_tolerance; limits.yaw_goal_tolerance = config.yaw_goal_tolerance; @@ -95,6 +99,7 @@ void AckermannPlannerROS::reconfigure_callback(iri_ackermann_local_planner::Acke limits.rot_stopped_vel = config.rot_stopped_vel; planner_util_.reconfigure_callback(limits, config.restore_defaults); this->last_cmds.resize(config.cmd_vel_avg); + this->use_steer_angle_cmd=config.use_steer_angle_cmd; odom_helper_.set_average_samples(config.odom_avg); patience_=config.planner_patience; @@ -437,34 +442,39 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) ackermann_msgs::AckermannDrive ackermann; odom_helper_.get_ackermann_state(ackermann); // steer the car without moving if the steering angle is big - - /* - if(fabs(ackermann.steer_angle-cmd_vel.angular.z)>0.05) + if(this->use_steer_angle_cmd) { - // IRI - ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steer_angle,cmd_vel.angular.z); - cmd_vel.linear.x=0.0; - - // EURECAT - //ROS_WARN("Setting forward speed to 0.1 (%f,%f)",ackermann.steer_angle,cmd_vel.angular.z); - //cmd_vel.linear.x=0.1; + if(fabs(ackermann.steering_angle-cmd_vel.angular.z)>0.05) + { + ROS_WARN("Setting forward speed to 0 (%f,%f)",ackermann.steering_angle,cmd_vel.angular.z); + cmd_vel.linear.x=0.0; + } + else + { + if(this->first) + this->first=false; + if(new_segment) + new_segment=false; + } } else { - */ - if(this->first) this->first=false; - if(new_segment) new_segment=false; - + if(this->first) + this->first=false; + if(new_segment) + new_segment=false; + } } - else if(fabs(cmd_vel.linear.x)<0.015) + else if(fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone) { +/* // get the position of the goal base_local_planner::getGoalPose(*tf_, transformed_plan, costmap_ros_->getGlobalFrameID(), goal_pose); // get the distance to the goal - double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.x); + double dist=base_local_planner::getGoalPositionDistance(current_pose_,goal_pose.pose.position.x,goal_pose.pose.position.y); if(dist>limits.xy_goal_tolerance && dist<2*limits.xy_goal_tolerance) { stucked_count++; @@ -490,6 +500,7 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) return false; } } +*/ } else { @@ -497,7 +508,16 @@ bool AckermannPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) replan_count=0; } if (isOk) + { publish_global_plan(transformed_plan); + if(limits.use_trans_vel_deadzone && fabs(cmd_vel.linear.x)<limits.trans_vel_deadzone) + { + if(cmd_vel.linear.x>0.0) + cmd_vel.linear.x=limits.trans_vel_deadzone; + else + cmd_vel.linear.x=-limits.trans_vel_deadzone; + } + } else { ROS_WARN_NAMED("ackermann_local_planner", "Ackermann planner failed to produce path."); diff --git a/src/ackermann_trajectory_generator.cpp b/src/ackermann_trajectory_generator.cpp index 498dd7d5fba5d69d8315db6e8b8e5fc75079fbd9..789c1bec6b67391fa0c1da31ac61dc6e83f0d332 100644 --- a/src/ackermann_trajectory_generator.cpp +++ b/src/ackermann_trajectory_generator.cpp @@ -248,6 +248,7 @@ void AckermannTrajectoryGenerator::set_parameters( double min_sim_time, double sim_granularity, double angular_sim_granularity, + bool use_steer_angle_cmd, double sim_period) { max_sim_time_ = max_sim_time; @@ -255,6 +256,7 @@ void AckermannTrajectoryGenerator::set_parameters( sim_granularity_ = sim_granularity; angular_sim_granularity_ = angular_sim_granularity; sim_period_ = sim_period; + use_steer_angle_cmd_ = use_steer_angle_cmd; } /** @@ -607,13 +609,24 @@ bool AckermannTrajectoryGenerator::generate_trajectory( time+=dt; } // end for simulation steps - if(fabs(sample_target_vel[1])>0.0) + if(this->use_steer_angle_cmd_) { - double radius=limits_->axis_distance/tan(sample_target_vel[1]); - traj.thetav_ = sample_target_vel[0]/radius; + traj.thetav_ = sample_target_vel[1]; + if(traj.thetav_>limits_->max_angular_vel) + traj.thetav_=limits_->max_angular_vel; + else if(traj.thetav_<limits_->min_angular_vel) + traj.thetav_=limits_->min_angular_vel; } else - traj.thetav_ = 0.0; + { + if(fabs(sample_target_vel[1])>0.0) + { + double radius=limits_->axis_distance/tan(sample_target_vel[1]); + traj.thetav_ = sample_target_vel[0]/radius; + } + else + traj.thetav_ = 0.0; + } return num_steps > 0; // true if trajectory has at least one point } diff --git a/src/heading_cost_function.cpp b/src/heading_cost_function.cpp index 41bc7c7babb8b000d5abe90499481f7e76f6f311..ad19be0c03c8f0e24d81ae61dfecd5ae5809b2a1 100644 --- a/src/heading_cost_function.cpp +++ b/src/heading_cost_function.cpp @@ -85,6 +85,7 @@ double HeadingCostFunction::scoreTrajectory(base_local_planner::Trajectory &traj diff=fabs(diff-3.14159); heading_diff+=diff; } + heading_diff/=this->num_points; return heading_diff; }