tutorial-ros-pioneer.cpp

example showing how to connect and send direct basic motion commands to a Pioneer mobile robot.

WARNING: this program does no sensing or avoiding of obstacles, the robot WILL collide with any objects in the way! Make sure the robot has about 2-3 meters of free space around it before starting the program.

#include <iostream>

#include <visp/vpTime.h>
#include <visp_ros/vpROSRobotPioneer.h>

int main(int argc, char **argv)
{
  try {
    std::cout << "\nWARNING: this program does no sensing or avoiding of obstacles, \n"
                 "the robot WILL collide with any objects in the way! Make sure the \n"
                 "robot has approximately 3 meters of free space on all sides.\n"
              << std::endl;

    vpROSRobotPioneer robot;

    robot.setCmdVelTopic("/RosAria/cmd_vel");
    robot.init();
    vpColVector v(2), v_mes(2);

    for (int i=0; i < 100; i++)
    {
      double t = vpTime::measureTimeMs();

      v = 0;
      v[0] = 0.01; // Translational velocity in m/s
      //v[1] = vpMath::rad(2); // Rotational velocity in rad/sec
      robot.setVelocity(vpRobot::REFERENCE_FRAME, v);

      vpTime::wait(t, 40);
    }

   return 0;
  }
  catch(vpException e) {
    std::cout << "Catch an exception: " << e << std::endl;
    return 1;
  }
}