controller.hpp

Go to the documentation of this file.

/*
 * Copyright (c) 2013, Marcus Liebhardt, Yujin Robot.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of Yujin Robot nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef YOCS_CONTROLLER_HPP_
#define YOCS_CONTROLLER_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/
#include <cmath>
#include <string>
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float32.h>
#include <tf/transform_listener.h>
#include <yocs_controllers/default_controller.hpp>

namespace yocs
{

class DiffDrivePoseController : public Controller
{
public:
  DiffDrivePoseController(ros::NodeHandle& nh, std::string& name) : Controller(),
                                                                       nh_(nh),
                                                                       name_(name){};
  virtual ~DiffDrivePoseController(){};

  bool init();

  void spinOnce();

private:
  bool getPoseDiff();
  void getControlOutput();
  void setControlOutput();

  void controlMaxVelCB(const std_msgs::Float32ConstPtr msg);

  void enableCB(const std_msgs::EmptyConstPtr msg);

  void disableCB(const std_msgs::EmptyConstPtr msg);

  // basics
  ros::NodeHandle nh_;
  std::string name_;

  // interfaces
  ros::Subscriber enable_controller_subscriber_;
  ros::Subscriber disable_controller_subscriber_;
  ros::Subscriber control_velocity_subscriber_;
  ros::Publisher command_velocity_publisher_;

  // variables and constants for the control law
  double r_;
  double theta_;
  double delta_;
  double v_;
  double v_max_;
  double w_;
  double w_max_;
  double cur_;
  double k_1_;
  double k_2_;
  double beta_;
  double lambda_;

  tf::TransformListener tf_listener_;
  tf::StampedTransform tf_goal_pose_rel_;
  std::string base_frame_name_;
  std::string goal_frame_name_;
};

bool DiffDrivePoseController::init()
{
  enable_controller_subscriber_ = nh_.subscribe("enable", 10, &DiffDrivePoseController::enableCB, this);
  disable_controller_subscriber_ = nh_.subscribe("disable", 10, &DiffDrivePoseController::disableCB, this);
  control_velocity_subscriber_ = nh_.subscribe("control_max_vel", 10, &DiffDrivePoseController::controlMaxVelCB, this);
  command_velocity_publisher_ = nh_.advertise<geometry_msgs::Twist>("command_velocity", 10);

  // retrieve configuration parameters
  base_frame_name_ = "base_footprint";
  if(!nh_.getParam("base_frame_name", base_frame_name_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'base_frame_name' from parameter server! Using default '"
                    << base_frame_name_ << "'. [" << name_ <<"]");
  }
  goal_frame_name_ = "base_goal_pose";
  if(!nh_.getParam("goal_frame_name", goal_frame_name_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'goal_frame_name' from parameter server! Using default '"
                    << goal_frame_name_ << "'. [" << name_ <<"]");
  }
  v_ = 0.0;
  v_max_ = 0.5;
  if(!nh_.getParam("v_max", v_max_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'v_max' from parameter server! Using default '"
                    << v_max_ << "'. [" << name_ <<"]");
  }
  w_max_ = M_PI / 4 * v_max_;
  k_1_ = 1.0;
  if(!nh_.getParam("k_1", k_1_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'k_1' from parameter server! Using default '"
                    << k_1_ << "'. [" << name_ <<"]");
  }
  k_2_ = 3.0;
  if(!nh_.getParam("k_2", k_2_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'k_2' from parameter server! Using default '"
                    << k_2_ << "'. [" << name_ <<"]");
  }
  beta_ = 0.4;
  if(!nh_.getParam("beta", beta_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'beta' from parameter server! Using default '"
                    << beta_ << "'. [" << name_ <<"]");
  }
  lambda_ = 2.0;
  if(!nh_.getParam("lambda", lambda_))
  {
    ROS_WARN_STREAM("Couldn't retrieve parameter 'lambda' from parameter server! Using default '"
                    << lambda_ << "'. [" << name_ <<"]");
  }
  ROS_DEBUG_STREAM("Controller initialised with the following parameters: [" << name_ <<"]");
  ROS_DEBUG_STREAM("base_frame_name = " << base_frame_name_ <<", goal_frame_name = "
                   << goal_frame_name_ << " [" << name_ <<"]");
  ROS_DEBUG_STREAM("v_max = " << v_max_ <<", k_1 = " << k_1_ << ", k_2 = " << k_2_
                   << ", beta = " << beta_ << ", lambda = " << lambda_ << " [" << name_ <<"]");
  return true;
};

void DiffDrivePoseController::spinOnce()
{
  if (this->getState())
  {
    ROS_INFO_STREAM_THROTTLE(1.0, "Controller spinning. [" << name_ <<"]");
    // determine pose difference in polar coordinates
    if (!getPoseDiff())
    {
      ROS_WARN_STREAM_THROTTLE(1.0, "Getting pose difference failed. Skipping control loop. [" << name_ <<"]");
      return;
    }
    // determine controller output (v, w)
    getControlOutput();
    // set control output (v, w)
    setControlOutput();
    // Logging
    ROS_DEBUG_STREAM_THROTTLE(1.0, "Current state: [" << name_ <<"]");
    ROS_DEBUG_STREAM_THROTTLE(1.0, "r = " << r_ << ", theta = " << theta_ << ", delta = " << delta_
                                   << " [" << name_ <<"]");
    ROS_DEBUG_STREAM_THROTTLE(1.0, "cur = " << cur_ << ", v = " << v_ << ", w = " << w_ << " [" << name_ <<"]");
  }
  else
  {
    ROS_WARN_STREAM_THROTTLE(3.0, "Controller is disabled. Idling ... [" << name_ <<"]");
  }
};

bool DiffDrivePoseController::getPoseDiff()
{
  // use tf to get information about the goal pose relative to the base
  try
  {
    tf_listener_.lookupTransform(base_frame_name_, goal_frame_name_, ros::Time(0), tf_goal_pose_rel_);
  }
  catch (tf::TransformException const &ex)
  {
    ROS_WARN_STREAM_THROTTLE(1.0, "Couldn't get transform from base to goal pose! [" << name_ <<"]");
    ROS_DEBUG_STREAM_THROTTLE(1.0, "tf error: " << ex.what());
    return false;
  }

  // determine distance to goal
  r_ = std::sqrt(std::pow(tf_goal_pose_rel_.getOrigin().getX(), 2)
                 + std::pow(tf_goal_pose_rel_.getOrigin().getY(), 2));
  // determine orientation of r relative to the base frame
  delta_ = std::atan2(-tf_goal_pose_rel_.getOrigin().getY(), tf_goal_pose_rel_.getOrigin().getX());
  // determine orientation of r relative to the goal frame
  // helper: theta = tf's orientation + delta
  theta_ = tf::getYaw(tf_goal_pose_rel_.getRotation()) + delta_;

  return true;
};

void DiffDrivePoseController::getControlOutput()
{
  cur_ = (-1 / r_) * (k_2_ * (delta_ - std::atan(-k_1_ * theta_))
         + (1 + (k_1_ / (1 + std::pow((k_1_ * theta_), 2)))) * sin(delta_));
  v_ = v_max_ / (1 + beta_ * std::pow(std::abs(cur_), lambda_));
  w_ = cur_ * v_;
  if (w_ > w_max_)
  {
    w_ = w_max_;
  }
};

void DiffDrivePoseController::setControlOutput()
{
  geometry_msgs::TwistPtr cmd_vel(new geometry_msgs::Twist());
  cmd_vel->linear.x = v_;
  cmd_vel->angular.z = w_;
  command_velocity_publisher_.publish(cmd_vel);
};

void DiffDrivePoseController::controlMaxVelCB(const std_msgs::Float32ConstPtr msg)
{
  v_max_ = msg->data;
  ROS_INFO_STREAM("Maximum linear control velocity has been set to " << v_max_ << ". [" << name_ << "]");
};

void DiffDrivePoseController::enableCB(const std_msgs::EmptyConstPtr msg)
{
  if (this->enable())
  {
    ROS_INFO_STREAM("Controller has been enabled. [" << name_ << "]");
  }
  else
  {
    ROS_INFO_STREAM("Controller was already enabled. [" << name_ <<"]");
  }
};

void DiffDrivePoseController::disableCB(const std_msgs::EmptyConstPtr msg)
{
  if (this->disable())
  {
    ROS_INFO_STREAM("Controller has been disabled. [" << name_ <<"]");
  }
  else
  {
    ROS_INFO_STREAM("Controller was already disabled. [" << name_ <<"]");
  }
};

} // namespace yocs

#endif /* YOCS_CONTROLLER_HPP_ */