skin_demo_movebase.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Bosch LLC
 * 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 Bosch LLC 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.
 */

//\Author Sebastian Haug, Bosch LLC

// Class
#include "skin_demo_movebase/skin_demo_movebase.h"


SkinDemoMovebase::SkinDemoMovebase()
        : buf_command_ (10, geometry_msgs::Twist())
{
        // Initialize private node handle
        nh_private_ = ros::NodeHandle("~");

        // Load parameter
        loadParameterInt(nh_private_,      std::string("num_sensors"),                  num_sensors_);

        loadParameterIntArray(nh_private_, std::string("mapping/fw"),                   patches_fw_);
        loadParameterIntArray(nh_private_, std::string("mapping/bw"),                   patches_bw_);
        loadParameterIntArray(nh_private_, std::string("mapping/right"),                patches_right_);
        loadParameterIntArray(nh_private_, std::string("mapping/left"),                 patches_left_);
        loadParameterIntArray(nh_private_, std::string("mapping/yawright"),     patches_yawright_);
        loadParameterIntArray(nh_private_, std::string("mapping/yawleft"),              patches_yawleft_);

        // Register publisher and subscriber
        sub_skin_data_ = nh_.subscribe("skin_data", 1, &SkinDemoMovebase::skinNewMeasCallback, this);
        pub_cmd_vel_ = nh_.advertise<geometry_msgs::Twist>("/base_controller/command", 1);
}

SkinDemoMovebase::~SkinDemoMovebase() {

}


void SkinDemoMovebase::skinNewMeasCallback(const boost::shared_ptr<skin_driver::skin_meas const>& msg) {
        // Check if num_sensors_ is correct
        if (num_sensors_ != (int)msg->sensor_number_msg)
                ROS_WARN("Number of sensors configured to '%i' but received measurement from '%i' sensors.", num_sensors_, (int)msg->sensor_number_msg);


        // ======== Convert low-level skin message

        // Vector of sensor_data_struct to store sensor data
        std::vector<sensor_data_struct> sensor_data(num_sensors_);

        // Convert received low-level data into higher-level sensor_data vector
        for(int i = 0; i < num_sensors_; i++)
        {
                sensor_data[i].stat     = ((255) & (msg->sensor_data_msg[i] >> (8*0)));
                sensor_data[i].x                = ((255) & (msg->sensor_data_msg[i] >> (8*1)));
                sensor_data[i].ini              = ((255) & (msg->sensor_data_msg[i] >> (8*2)));
                sensor_data[i].limit    = ((255) & (msg->sensor_data_msg[i] >> (8*3)));
                sensor_data[i].u1               = ((255) & (msg->sensor_data_msg[i] >> (8*4)));
                sensor_data[i].u2               = ((255) & (msg->sensor_data_msg[i] >> (8*5)));
                sensor_data[i].u3               = ((255) & (msg->sensor_data_msg[i] >> (8*6)));
                sensor_data[i].dyn              = ((255) & (msg->sensor_data_msg[i] >> (8*7)));
        }

//      // DEBUG
//      ROS_INFO("Message received:");
//      for(int i = 0; i < (int)sensor_data.size(); i++){
//              //ROS_INFO("    Sensor %i: x: %i, ini: %i", i, sensor_data[i].x, sensor_data[i].ini);
//              ROS_INFO("    Sensor %i: val: %i", i, abs(sensor_data[i].x - sensor_data[i].ini));
//      }


        // ======== Match patches to drive/turn actions and calculate drive values

        // The maximum value of all patches belonging to a specific action (e.g. driving
        // forward) is determined by iterating through the corresponding vector (e.g. patches_fw_)
        // containing the ids of patches belonging to that action and looking up the x and ini values
        // in sensor_data.
    std::vector<int>::iterator it;
    double drive_fw = 0, drive_left     = 0, drive_right = 0, drive_bw = 0;
    double yaw_right = 0, yaw_left = 0;

    it = patches_fw_.begin();
    while (it != patches_fw_.end()) {
      drive_fw = std::max(drive_fw, (double)abs(sensor_data[*it].x - sensor_data[*it].ini));
      ++it;
//      ROS_INFO("fw: %f", drive_fw);
    }

    it = patches_bw_.begin();
    while (it != patches_bw_.end()) {
      drive_bw = std::max(drive_bw, (double)abs(sensor_data[*it].x - sensor_data[*it].ini));
      ++it;
//      ROS_INFO("bw: %f", drive_bw);
    }

    it = patches_right_.begin();
    while (it != patches_right_.end()) {
      drive_right = std::max(drive_right, (double)abs(sensor_data[*it].x - sensor_data[*it].ini));
      ++it;
//      ROS_INFO("right: %f", drive_right);
    }

    it = patches_left_.begin();
    while (it != patches_left_.end()) {
      drive_left = std::max(drive_left, (double)abs(sensor_data[*it].x - sensor_data[*it].ini));
      ++it;
//      ROS_INFO("left: %f", drive_left);
    }

    it = patches_yawleft_.begin();
    while (it != patches_yawleft_.end()) {
        yaw_left = std::max(yaw_left, (double)abs(sensor_data[*it].x - sensor_data[*it].ini));
      ++it;
//      ROS_INFO("yawleft: %f", yaw_left);
    }

    it = patches_yawright_.begin();
    while (it != patches_yawright_.end()) {
        yaw_right = std::max(yaw_right, (double)abs(sensor_data[*it].x - sensor_data[*it].ini));
      ++it;
//      ROS_INFO("yawright: %f", yaw_right);
    }


    // ======== Convert drive/turn actions to velocities in m/s or rad/s

        // Calculate velocity in m/s
        //                           .------------------------------. -> calibrates skin values to range approx from 0 to 1
        double vel_fw           = 0.20 * (((double)drive_fw     -5) / 50);
        double vel_bw           = 0.20 * (((double)drive_bw     -5) / 50);
        double vel_right        = 0.20 * (((double)drive_right  -5) / 50);
        double vel_left         = 0.20 * (((double)drive_left   -5) / 50);
        double vel_yawleft      = 0.20 * (((double)yaw_left     -5) / 50);
        double vel_yawright = 0.20 * (((double)yaw_right        -5) / 50);

    // Enforce safety -> 0.25 m/s is max velocity
        vel_fw                  = std::min(std::max(vel_fw, 0.0), 0.25);
        vel_bw                  = std::min(std::max(vel_bw, 0.0), 0.25);
        vel_right               = std::min(std::max(vel_right, 0.0), 0.25);
        vel_left                = std::min(std::max(vel_left, 0.0), 0.25);
        vel_yawleft             = std::min(std::max(vel_yawleft, 0.0), 0.25);
        vel_yawright    = std::min(std::max(vel_yawright, 0.0), 0.25);


        // ======== Assign velocities to twist

    // We will send commands of type "twist"
        geometry_msgs::Twist base_cmd;
        base_cmd.linear.x = base_cmd.linear.y = base_cmd.angular.z = 0;

        // Threshold (between ini and x) which has to be achieved before sensor excitation value is
        // interpreted as action
        int action_thresh = 5;

        bool new_command = false;

        // Move back
        if (drive_bw > action_thresh)
        {
                ROS_INFO("MOVE: back,    vel: %.2f raw sensor value abs(x-ini): %f,", vel_bw, drive_bw);
                base_cmd.linear.x = -vel_bw;
                new_command = true;
        }

        // Move forward - overwrites back
        if (drive_fw > action_thresh)
        {
                ROS_INFO("MOVE: forward, vel: %.2f, raw sensor value abs(x-ini): %f", vel_fw, drive_fw);
                base_cmd.linear.x = vel_fw;
                new_command = true;
        }

        // Move left
        if (drive_left > action_thresh)
        {
                ROS_INFO("MOVE: left,    vel: %.2f, raw sensor value abs(x-ini): %f", vel_left, drive_left);
                base_cmd.linear.y = vel_left;
                new_command = true;
        }

        // Move right - overwrites left
        if (drive_right > action_thresh)
        {
                ROS_INFO("MOVE: right,   vel: %.2f, raw sensor value abs(x-ini): %f", vel_right, drive_right);
                base_cmd.linear.y = -vel_right;
                new_command = true;
        }

        // Yaw right
        if (yaw_right > action_thresh)
        {
                ROS_INFO("TRUN: right,   vel: %.2f, raw sensor value abs(x-ini): %f", vel_yawright, yaw_right);
                base_cmd.angular.z = -vel_yawright;
                new_command = true;
        }

        // Yaw left - overwrites yaw left
        if (yaw_left > action_thresh)
        {
                ROS_INFO("TRUN: left,   vel: %.2f, raw sensor value abs(x-ini): %f", vel_yawleft, yaw_left);
                base_cmd.angular.z = vel_yawleft;
                new_command = true;
        }


        // ======= Push command to circular buffer

        // Pushed 4 times as skin runs @5Hz only, but we want to publish @20Hz
        if (new_command) {
                for (int c = 0; c < 4; c++)
                {
                        buf_command_.push_front(base_cmd);
                }
        }
}


void SkinDemoMovebase::publishBaseCommand() {
        ros::Rate loop_rate(20);
        geometry_msgs::Twist base_cmd;

        // Clear buffer
        buf_command_.clear();

        while (ros::ok()) {
                // Create base_cmd
                if (! buf_command_.empty()) {
                        // Get next cmd from buffer
                        base_cmd = buf_command_.front();
                        buf_command_.pop_front();
                }
                else {
                        // Reset base_cmd
                        base_cmd.linear.x = 0;
                        base_cmd.linear.y = 0;
                        base_cmd.linear.z = 0;
                        base_cmd.angular.x = 0;
                        base_cmd.angular.y = 0;
                        base_cmd.angular.z = 0;
                }

                // Publish base_cmd
                pub_cmd_vel_.publish(base_cmd);

                // Spin and loop_rate
                ros::spinOnce();
                loop_rate.sleep();
        }
}


void SkinDemoMovebase::loadParameterInt(ros::NodeHandle & nh_, std::string param, int & data)
{
        bool param_available = nh_.hasParam(param);
        if (!param_available)
        {
                ROS_ERROR("Parameter '%s' not available. Aborting...", param.c_str());
                ros::shutdown();
                exit(1);
        }

        nh_.getParam(param.c_str(), data);
}


void SkinDemoMovebase::loadParameterIntArray(ros::NodeHandle & nh_, std::string param, std::vector<int> & vec)
{
        bool param_available = nh_.hasParam(param);
        if (!param_available)
        {
                ROS_ERROR("Parameter '%s' not available. Aborting...", param.c_str());
                ros::shutdown();
                exit(1);
        }

        XmlRpc::XmlRpcValue raw;
        nh_.getParam(param.c_str(), raw);
        ROS_ASSERT(raw.getType() == XmlRpc::XmlRpcValue::TypeArray);
        for (int32_t i = 0; i < raw.size(); ++i)
        {
                ROS_ASSERT(raw[i].getType() == XmlRpc::XmlRpcValue::TypeInt);
                vec.push_back(static_cast<int>(raw[i]));
        }
}


int main(int argc, char** argv) {
        // Initialize node
        ros::init(argc, argv, "skin_demo_movebase");

        //ros::spin();

        // Create object and spin in publishBaseCommand
        SkinDemoMovebase skin_demo_movebase;
        skin_demo_movebase.publishBaseCommand();
}

---

skin_demo_movebase
Author(s): Sebastian Haug
autogenerated on Fri Jan 11 09:53:57 2013