Change trajectory cost function in base_local_planner
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.
