control_plugin.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include <math.h>
#include <angles/angles.h>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/mutex.hpp>

#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <realtime_tools/realtime_publisher.h>

#include <cob_tricycle_controller/param_parser.h>
#include <cob_base_controller_utils/WheelCommands.h>
#include <geometry_msgs/Twist.h>

#include <cob_tricycle_controller/TricycleCtrlTypes.h>

#include <pluginlib/class_list_macros.h>

namespace cob_tricycle_controller
{

double limitValue(double value, double limit){
    if(limit != 0){
        if (value > limit){
                value = limit;
        } else if (value < -limit) {
                value = -limit;
        }
    }
    return value;
}


class WheelController : public controller_interface::MultiInterfaceController<hardware_interface::VelocityJointInterface, hardware_interface::PositionJointInterface>
{
public:
    virtual bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle &nh)
    {
        if (!nh.getParam("steer_joint", wheel_state_.steer_name)){
            ROS_ERROR("Parameter 'steer_joint' not set");
            return false;
        }
        if (!nh.getParam("drive_joint", wheel_state_.drive_name)){
            ROS_ERROR("Parameter 'drive_joint' not set");
            return false;
        }

        hardware_interface::VelocityJointInterface* v = robot_hw->get<hardware_interface::VelocityJointInterface>();
        hardware_interface::PositionJointInterface* p = robot_hw->get<hardware_interface::PositionJointInterface>();
        steer_joint_ = p->getHandle(wheel_state_.steer_name);
        drive_joint_ = v->getHandle(wheel_state_.drive_name);

        urdf::Model model;
        std::string description_name;
        bool has_model = nh.searchParam("robot_description", description_name) &&  model.initParam(description_name);

        urdf::Vector3 steer_pos;
        urdf::JointConstSharedPtr steer_joint;
        urdf::JointConstSharedPtr drive_joint;

        if(has_model){
            steer_joint = model.getJoint(wheel_state_.steer_name);
            if(steer_joint){
                tf2::Transform transform;
                //root link for tricycle is "base_pivot_link"
                if(parseWheelTransform(wheel_state_.steer_name, model.getRoot()->name, transform, &model)){
                    wheel_state_.pos_x = transform.getOrigin().getX();
                    wheel_state_.pos_y = transform.getOrigin().getY();
                    wheel_state_.radius = transform.getOrigin().getZ();
                    wheel_state_.sign = cos(transform.getRotation().getAngle());
                }
                nh.param("max_steer_rate", max_steer_rate_, 0.0);
                max_steer_rate_ = (max_steer_rate_ != 0.0 && max_steer_rate_ < steer_joint->limits->velocity) ? max_steer_rate_ : steer_joint->limits->velocity;
            }
            drive_joint = model.getJoint(wheel_state_.drive_name);
            if(drive_joint){
                nh.param("max_drive_rate", max_drive_rate_, 0.0);
                max_drive_rate_ = (max_drive_rate_ != 0.0 && max_drive_rate_ < drive_joint->limits->velocity) ? max_drive_rate_ : drive_joint->limits->velocity;
            }
        }

        nh.param("max_trans_velocity", max_vel_trans_, 0.0);
        if(max_vel_trans_ < 0){
            ROS_ERROR_STREAM("max_trans_velocity must be non-negative.");
            return false;
        }
        nh.param("max_rot_velocity", max_vel_rot_, 0.0);
        if(max_vel_rot_ < 0){
            ROS_ERROR_STREAM("max_rot_velocity must be non-negative.");
            return false;
        }
        double timeout;
        nh.param("timeout", timeout, 1.0);
        if(timeout < 0){
            ROS_ERROR_STREAM("timeout must be non-negative.");
            return false;
        }
        timeout_.fromSec(timeout);

        pub_divider_ =  nh.param("pub_divider", 0);
        twist_subscriber_ = nh.subscribe("command", 1, &WheelController::topicCallbackTwistCmd, this);
        commands_pub_.reset(new realtime_tools::RealtimePublisher<cob_base_controller_utils::WheelCommands>(nh, "wheel_commands", 5));

        commands_pub_->msg_.drive_target_velocity.resize(1);
        commands_pub_->msg_.steer_target_velocity.resize(1);
        commands_pub_->msg_.steer_target_position.resize(1);
        commands_pub_->msg_.steer_target_error.resize(1);

        return true;
    }

    virtual void starting(const ros::Time& time){
        cycles_ = 0;
    }

    virtual void update(const ros::Time& time, const ros::Duration& period)
    {
        {
            boost::mutex::scoped_try_lock lock(mutex_);
            if(lock){
                Target target = target_;
                target_.updated = false;

                if(!target.stamp.isZero() && !timeout_.isZero() && (time - target.stamp) > timeout_){
                    ROS_WARN_STREAM("target timed out");
                    target_.stamp = ros::Time(); // only reset once
                    target.state  = PlatformState();
                    target.updated = true;
                }
                lock.unlock();
            }
        }

        updateCommand();

        steer_joint_.setCommand(wheel_command_.steer_pos);
        drive_joint_.setCommand(wheel_command_.drive_vel);

        if(cycles_ < pub_divider_ && (++cycles_) == pub_divider_){
            if(commands_pub_->trylock()){
                ++(commands_pub_->msg_.header.seq);
                commands_pub_->msg_.header.stamp = time;

                commands_pub_->msg_.drive_target_velocity[0] = wheel_command_.drive_vel;
                commands_pub_->msg_.steer_target_velocity[0] = 0.0;
                commands_pub_->msg_.steer_target_position[0] = wheel_command_.steer_pos;
                commands_pub_->msg_.steer_target_error[0] = wheel_command_.steer_pos - wheel_state_.steer_pos;
                commands_pub_->unlockAndPublish();
            }
            cycles_ = 0;
        }
    }

private:
    struct Target {
        PlatformState state;
        bool updated;
        ros::Time stamp;
    } target_;

    WheelState wheel_state_;
    WheelState wheel_command_;
    hardware_interface::JointHandle steer_joint_;
    hardware_interface::JointHandle drive_joint_;

    boost::mutex mutex_;
    ros::Subscriber twist_subscriber_;

    boost::scoped_ptr<realtime_tools::RealtimePublisher<cob_base_controller_utils::WheelCommands> > commands_pub_;
    uint32_t cycles_;
    uint32_t pub_divider_;

    ros::Duration timeout_;
    double max_vel_trans_, max_vel_rot_;
    double max_steer_rate_, max_drive_rate_;

    void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg){
        if(this->isRunning()){
            boost::mutex::scoped_lock lock(mutex_);
            if(isnan(msg->linear.x) || isnan(msg->linear.y) || isnan(msg->angular.z)) {
                ROS_FATAL("Received NaN-value in Twist message. Reset target to zero.");
                target_.state = PlatformState();
            }else{
                target_.state.velX = limitValue(msg->linear.x, max_vel_trans_);
                target_.state.velY = limitValue(msg->linear.y, max_vel_trans_);
                target_.state.rotTheta = limitValue(msg->angular.z, max_vel_rot_);
            }
            target_.updated = true;
            target_.stamp = ros::Time::now();
        }
    }

    void updateCommand(){
        //get JointState from JointHandles
        wheel_state_.steer_pos = steer_joint_.getPosition();
        wheel_state_.steer_vel = steer_joint_.getVelocity();
        wheel_state_.drive_pos = drive_joint_.getPosition();
        wheel_state_.drive_vel = drive_joint_.getVelocity();

        // calculate inverse kinematics
        // http://www.wolframalpha.com/input/?i=Solve%5Bx%3D%3Dw*cos(a),+phi%3D%3Dw*sin(a)%2Fr,a,w%5D
        double r_base = wheel_state_.pos_x * wheel_state_.sign;
        double k = sqrt(pow(r_base,2)*pow(target_.state.rotTheta,2) + pow(target_.state.velX,2));
        if (target_.state.rotTheta != 0)
        {
            double a1 = 2 * atan2(k - target_.state.velX, r_base*target_.state.rotTheta);
            a1 = angles::normalize_angle(a1);
            double b1 = k / wheel_state_.radius;

            double a2 = -2 * atan2(k + target_.state.velX, r_base*target_.state.rotTheta);
            a2 = angles::normalize_angle(a2);
            double b2 = - k / wheel_state_.radius;

            if (fabs(a1-wheel_state_.steer_pos) <= fabs(a2-wheel_state_.steer_pos))
            {
                wheel_command_.steer_pos = a1;
                wheel_command_.drive_vel = b1;
            }
            else
            {
                wheel_command_.steer_pos = a2;
                wheel_command_.drive_vel = b2;
            }
        }
        else if (target_.state.velX != 0)
        {
            if (fabs(0.0-wheel_state_.steer_pos) <= fabs(M_PI-wheel_state_.steer_pos))
            {
                wheel_command_.steer_pos = 0.0;
                wheel_command_.drive_vel = target_.state.velX / wheel_state_.radius;
            }
            else
            {
                wheel_command_.steer_pos = M_PI;
                wheel_command_.drive_vel = -target_.state.velX / wheel_state_.radius;
            }
        }
        else {
            wheel_command_.steer_pos = wheel_state_.steer_pos;
            wheel_command_.drive_vel = 0.0;
        }

        wheel_command_.drive_vel = limitValue(wheel_command_.drive_vel, max_drive_rate_);
    }
};

}

PLUGINLIB_EXPORT_CLASS( cob_tricycle_controller::WheelController, controller_interface::ControllerBase)