Path Planner RamseteAutoBuilder in C++ Robot Crashing

I am trying to implement the Path Planner RamseteAutoBuilder into my project, but am having some issues. When I attempt to run auto, the program seems to crash and then restart. There are also no messages in the console when this occurs. I talked to Michael the developer of Path Planner and we could not come up with a solution yet. I was hoping somebody else might have an idea. Here is the code in the GetAutonomousCommand for Path Planner and the code in AutonomousInit as well.

frc2::CommandPtr RobotContainer::GetAutonomousCommand() {
    std::vector<pathplanner::PathPlannerTrajectory> traj = pathplanner::PathPlanner::loadPathGroup(m_autoChooser.GetSelected(), {pathplanner::PathConstraints(4_mps, 3_mps_sq)});
    
    pathplanner::RamseteAutoBuilder ramseteCommand{[this]() {return m_drivetrain.GetPose();}, 
                                                   [this] (frc::Pose2d pose) {m_drivetrain.ResetOdometry(pose);}, 
                                                   frc::RamseteController{DriveConstants::kRamseteB * 1_rad * 1_rad / (1_m * 1_m),
                                                                          DriveConstants::kRamseteZeta / 1_rad}, 
                                                   DriveConstants::kDriveKinematics, 
                                                   frc::SimpleMotorFeedforward<units::meters>{DriveConstants::kS, DriveConstants::kV, DriveConstants::kA}, 
                                                   [this] {return m_drivetrain.GetWheelSpeed();},
                                                   {DriveConstants::kPDriveVel, 0, 0},
                                                   [this] (auto left, auto right) {m_drivetrain.DriveVolts(left, right);},
                                                   eventMap,
                                                   {&m_drivetrain}, false};
    return ramseteCommand.fullAuto(traj);

It’s worth noting that m_autoChooser pulls from a list of strings that are selected via the dashboard to choose the desired autonomous.

void Robot::AutonomousInit() {
  m_autonomousCommand = m_container.GetAutonomousCommand();
  if (m_autonomousCommand) {
    m_autonomousCommand->Schedule();
  }
}

Nothing is in the RioLog? Or are you talking about the DS console?

You might try doing remote debugging (wpilib debug robot code) to help find the crash.

As far as I can tell, the issue is likely that you’re letting the RamseteAutoBuilder go out of scope. From its implementation, there are “this” captures inside of it (e.g. in BaseAutoBuilder::resetPose()), so you need to not let it go out of scope as long as the returned command exists.

What’s the best way of making sure it stays in scope?

Both the DS console and the RioLog.

Make it a member variable of the container.

That seemed to fix it. Thanks!

We are having the exact same issue and are using a very similar implementation in regards to the auto chooser.

I need help with the statement “Make it a member variable of the container”

What exactly should we be making a member variable?

Do we need to build a vector of pointers to auto commands for every possible auto option and then save them in the robotcontainer as the robot container is being constructed and then have the AutoInit look at the value chosen on the dashboard and schedule the appropriate auto command from this?

@CosmicROM How did you fix this problem?

The member variable should be the pathplanner::RamseteAutoBuilder. I also have the event map defined as a member variable. As for auto commands, I just pull a string from the dashboard and use that when I load the path group as the filename of the path. Then you can use this path group as a trajectory for the Auto Builder. Hopefully that helps. Let me know if anything needs more clarification.

Thanks for your help, just a another quick question.

I think I’m good with the event map stuff
In the header
std::unordered_map<std::string, std::shared_ptr<frc2::Command>> m_eventMap;

Then in the constructor code between the {} add these.

  m_eventMap.emplace("marker1", std::make_shared<frc2::PrintCommand>("Passed Marker 1"));
  m_eventMap.emplace("marker2", std::make_shared<frc2::PrintCommand>("Passed Marker 2"));
  m_eventMap.emplace("marker3", std::make_shared<frc2::PrintCommand>("Passed Marker 3"));
  m_eventMap.emplace("marker4", std::make_shared<frc2::PrintCommand>("Passed Marker 4"));

But for more complex things I’m never real sure on the correct way to do this.

What do you do with the RamseteAutoBuilder object?

Did you declare the RamseteAutoBuilder object as a member variable with no parameters in the header for the RobotContainer and then given it everything the RamseteAutoBuilder needs in the robot container constructor?

If this is the case, do all the parameter to the RamsetAutoBuilder object go in the section after the “:” but before the {} in the .cpp for the robotcontainer?

Or does the whole RamseteAutoBuilder object with all its parameters, go in the robotcontainer header as part of the class definition?

In my case it’s a ServeAutoBuilder object instead of a RamseteAutoBuilder object but I’m assuming it will all be same. I had the exact same problem as you.

Thanks again for your help.

I declared the AutoBuilder in the header file with no parameters and then put all the parameters after the “:” in RobotContainer. I also had the event map all defined in the header instead of using the emplace function so that it can be called into the AutoBuilder when the parameters are added in RobotContainer. Unfortunately due to the mechanical portions of the robot not being completed I haven’t been able to completely test the functionality, but it did keep it in scope so Auto does not just crash every time so it is at least a step in the right direction. Definitely update me if this works for you.

What syntax did you use for this ?

I also had the event map all defined in the header instead of using the emplace function

Something along these lines.

  std::unordered_map<std::string, std::shared_ptr<frc2::Command>> eventMap = 
              {{"Command Name 1", std::make_shared<CommandFileName1>(command inputs)},
              {{"Command Name 2", std::make_shared<CommandFileName2>(command inputs)}};

This is just a basic example but hopefully it makes sense. I don’t currently have any PrintCommands in mine but adding them would be the same syntax as you have before.

Cool,

That makes perfect sense. We had to copy the pathplanner code locally and modify the source code to remove the setPose stuff in order to stop it crashing the RoboRio.

After reading this I now understand why it was crashing the code.

We will try to go back to using the online version of path plannner and then make the changes as suggested.

I’ll report back if it all works after this weekend.

Cheers
Warren

No Luck yet, we tried for hours yesterday trying to get this to work.

I’m fully aware that it’s not going to be a problem with pathplanner. Its just going to be our implementation that is breaking things.

As soon as we move the swerveAutoBuilder object into the robot container as a member variable we get really weird behaviour.

It wont run the first time after a reboot and then when it runs on the second attempt, it’s all jerky.
We have no idea what is causing this at the moment.

We have tried creating it as a member variable to the robotContainer and then initialize it after the : in the constructor.

We have even tried having the member variable be a pointer to a SwerveAutoBuilder and creating it with a m_autoBuilder = new … inside the { } section of the constructor.

Both implementations aren’t working for us.

If anyone has a working c++ implementation with a working auto selector I’d love to see the source code of the RobotContainer.cpp, RobotContainer.h, Robot.cpp and Robot.h

From the advice on this post it appears that if you let the object that creates the SwerveAutoBuilder object go out of scope, it breaks because of the lambda capture of [this] in the setpose and getpose parameters. But the wpilib example code for swerve controller command does exactly this. They create a Swerve Controller Command object in their GetAutonomousCommand that uses [this] captures to create the SwerveControlCommand object and GetAutonomousCommand goes out of scope before the command that it creates is executed. (in AutoInit).

I wish I fully understood passing functions as parameters using lambdas that capture [this] on methods that go out of scope.

1 Like

The this captures in the SwerveControllerCommand example’s GetAutonomousCommand() capture the RobotContainer object, which stays in scope throughout the robot program.

Methods don’t go out of scope, objects do. In the WPILib example case, using this in RobotContainer::GetAutonomousCommand() means when you call m_container->GetAutonomousCommand(), m_container is what is captured (this == m_container). This is only a problem if m_container goes out of scope and is destroyed before one of those std::function’s created in GetAutonomousCommand is called and dereferences the captured this.

Thanks Peter,

Methods don’t go out of scope, objects do. In the WPILib example case, using this in RobotContainer::GetAutonomousCommand() means when you call m_container->GetAutonomousCommand() , m_container is what is captured (this == m_container ). This is only a problem if m_container goes out of scope and is destroyed before one of those std::function’s created in GetAutonomousCommand is called and dereferences the captured this .

That makes a lot more sense now that it is explained like that.

So if there is a class method that executes, and it has lambdas in it that capture [this], the method can be called from somewhere else in the program, run and finish and return whatever object it returns and everything should be all good as long as the instance of class that the method belongs to, remains in scope.

And the only real way to make sure an instance of a class object stays in scope for the life of a robot match is to make sure that the object is a member variable of one of the major classes eg robot, robotcontainer, subsystem, etc.

If that is correct, the next question is, when you want this object to remain in scope it has to be a member variable.
So you include the member variable declaration in the .h that holds the class definition like this?

ClassType m_memberVariableName;

But what is the best way to supply the arguments required to make the object?
Should it be in the classes .cpp file after the

:

but before the

{
//constructor code here
}

And if the parameters are supplied here and some of the parameter contains lambda that use [this] Is this valid?

On a side note and getting back to the initial problem

in regards to

As soon as we move the swerveAutoBuilder object into the robot container as a member variable we get really weird behaviour.

It wont run the first time after a reboot and then when it runs on the second attempt, it’s all jerky.
We have no idea what is causing this at the moment.

We have tried creating it as a member variable to the robotContainer and then initialize it after the : in the constructor.

We found the issue that was causing the weird behavior. After adding a bunch of logging we could see the speeds being applied to the robot while following a path, were continuously alternating between the required speeds and zero. It turned out that the drive subsystem wasn’t being “required” by the command group returned from pathplanners fullauto method. So the default command for the drive subsystem was running ever execution cycle. So it was going at the required speed, then at the joystick values and then at the required speed and then joystick values etc etc. The zero speeds from the joystick were pretty much preventing the robot moving so the path errors were progressively getting larger and larger and the jitters got more violent fairly quickly. Our quick work around was to add the command group generated by the fullauto method to another sequential command group that did “require” the drive subsystem. When we launched this command the drive subsystem was being “required” and this prevented the drive subsystem default command from running and the path following started working.

I’ll have to go digging down through pathplanners implementation to see why the command requirements aren’t working when we make the autobuilder a member variable of the robot container.

I’m guessing it’s probably another [this] capture issue deep down in the pathplanner software.

Thanks again Peter for your help in understanding c++.

1 Like

Yes.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.