Change trajectory cost function in base_local_planner

asked 2016-09-02 15:35:06 -0500

curranw gravatar image

updated 2016-09-02 22:24:05 -0500

I have some x,y,theta,cost data as "human feedback" for a planning algorithm. I'd like the robot to navigate while taking these human preferences into account.

However, base_local_planner uses 2D costmaps. I can easily use the x,y data, but then there is no differentiation between different thetas. I can add an additional layer with theta, but that's not really what I want. Ideally, I'd like to change the trajectory scoring to take into account human preference. For example, if the trajectory goes through many points that the human feedback deems bad, give that trajectory a low score.

Right now my plan is to check out the navigation stack and add my additional trajectory costs. Is there a better way to do this?

Edit: I checked out the base_local_planner and modified the TrajectoryPlanner::generateTrajectory function. I added my human feedback to the costs. Now it throws out bad trajectories, but it either still navigates through the bad x,y,theta coordinates, or has to perform a recovery action.

edit retag flag offensive close merge delete