core_wrapper.cpp

Go to the documentation of this file.

#include "roch_base/core/sawyer.h"
#include <ros/ros.h>

namespace
{
  std::string port_;
}

namespace core
{

  void reconnect()
  {
    if (port_.empty())
    {
      throw std::logic_error("Can't reconnect when port is not configured");
    }
    ROS_INFO_STREAM("Connecting to roch on port " << port_ << "...");
    sawyer::Transport::instance().configure(port_.c_str(), 3);
  }

  void connect(std::string port)
  {
    port_ = port;
    reconnect();
  }

  void configureLimits(double max_speed, double max_accel)
  {

    bool success = false;
    while (!success)
    {
      try
      {
        
        sawyer::SetMaxAccel(max_accel, max_accel).send();
        sawyer::SetMaxSpeed(max_speed, max_speed).send();
        success = true;
      }
      catch (sawyer::Exception *ex)
      {
        ROS_ERROR_STREAM("Error configuring velocity and accel limits: " << ex->message);
        reconnect();
      }
    }
  }

  void controlSpeed(double speed_left, double speed_right, double accel_left, double accel_right)
  {
    bool success = false;
    while (!success)
    {
      try
      {
        sawyer::SetDifferentialSpeed(speed_left, speed_right, accel_left, accel_right).send();
        success = true;
      }
      catch (sawyer::Exception *ex)
      {
        ROS_ERROR_STREAM("Error sending speed and accel command: " << ex->message);
        reconnect();
      }
    }
  }
    void controloverallSpeed(double speed_left, double speed_right, double accel_left, double accel_right)
  {
    bool success = false;
    double lin_vel = (speed_left + speed_right)*0.5;
    double ang_vel = (speed_left - speed_right)/(2*0.12);
    
    double acc = (accel_left + accel_right)*0.5;
    
    while (!success)
    {
      try
      {
        sawyer::SetVelocity(lin_vel,ang_vel,acc).send();
        success = true;
      }
      catch (sawyer::Exception *ex)
      {
        ROS_ERROR_STREAM("Error sending velocity setpt command: " << ex->message);
        reconnect();
      }
    }
  }

}