Issue with setting speed to DifferentialWheels in Webots C++

Question

Small community here, but hopefully somebody sees this. I'm attempting to do a pure C++ implementation of a Webots simulation for an E-puck. The C++ documentation is sorely lacking, and I can't seem to find a resolution for this issue (the C implementation is stellar, but all the function calls were changed for C++).

Essentially, I'm just trying to get a simple application up and running...I want to make the E-puck move forward. I will post the entirety of my code below...all I'm doing is instantiating a Robot entity, printing out all the IR sensor values, and attempting to move it forward.

The issue is that it does not move. I'd think that there would be some call to link the DifferentialWheel object to the E-puck (similar to the camera = getCamera("camera") call).

If I comment out my call to setSpeed, the program works perfectly (doesn't move, but prints values). If I leave it in, the simulation freezes up after a single step, once it gets to that call. I'm not exactly sure what I'm doing wrong, to be honest.

// webots
#include <webots/Robot.hpp>
#include <webots/Camera.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/DifferentialWheels.hpp>
#include <webots/LED.hpp>

// standard
#include <iostream>

using namespace webots;

#define TIME_STEP 16

class MyRobot : public Robot
{
  private:
    Camera *camera;
    DistanceSensor *distanceSensors[8];
    LED *leds[8];
    DifferentialWheels *diffWheels;

  public: 
    MyRobot() : Robot()
    {
      // camera
      camera             = getCamera("camera");

      // sensors
      distanceSensors[0] = getDistanceSensor("ps0");
      distanceSensors[1] = getDistanceSensor("ps1");
      distanceSensors[2] = getDistanceSensor("ps2");
      distanceSensors[3] = getDistanceSensor("ps3");
      distanceSensors[4] = getDistanceSensor("ps4");
      distanceSensors[5] = getDistanceSensor("ps5");
      distanceSensors[6] = getDistanceSensor("ps6");
      distanceSensors[7] = getDistanceSensor("ps7");
      for (unsigned int i = 0; i < 8; ++i)
        distanceSensors[i]->enable(TIME_STEP);

      // leds
      leds[0] = getLED("led0");
      leds[1] = getLED("led1");
      leds[2] = getLED("led2");
      leds[3] = getLED("led3");
      leds[4] = getLED("led4");
      leds[5] = getLED("led5");
      leds[6] = getLED("led6");
      leds[7] = getLED("led7");
    }

    virtual ~MyRobot()
    {
      // cleanup
    }

    void run()
    {
      double speed[2] = {20.0, 0.0};

      // main loop
      while (step(TIME_STEP) != -1)
      {
        // read sensor values
        for (unsigned int i = 0; i < 8; ++i)
          std::cout << " [" << distanceSensors[i]->getValue() << "]";
        std::cout << std::endl;

        // process data

        // send actuator commands
// this call kills the simulation
//        diffWheels->setSpeed(1000, 1000);

      }
    }
};

int main(int argc, char* argv[])
{

  MyRobot *robot = new MyRobot();
  robot->run();
  delete robot;
  return 0;
}

Now, if this were the C implementation, I would call wb_differential_wheels_set_speed(1000, 1000); However, that call isn't available in the C++ header files.

Answer 1

It doesn't seem as though you've initialized diffWheels, so I would imagine you're getting a segfault from dereferencing a garbage pointer. Try putting

diffWheels = new DifferentialWheels;

in the constructor of MyRobot.

Answer 2

The problem causing the freeze is due to the use of the uninitialized variable diffWheels. DifferentialWheels (as well as Robot and Supervisor) doesn't need to be initialized.

You have to change the base class of your MyRobot class to DifferentialWheels

class MyRobot : public DifferentialWheels

and then simply call

setSpeed(1000, 1000)

and not

diffWheels->setSpeed(1000, 1000)