skin_safe_base.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_safe_base/skin_safe_base.h"


SkinSafeBase::SkinSafeBase()
{
        // Initialize private node handle
        nh_private_ = ros::NodeHandle("~");

        // Load parameter
        loadParameterInt(nh_private_,      std::string("num_sensors"),          num_sensors_);
        loadParameterInt(nh_private_,      std::string("sensor_threshold"), sensor_threshold_);
        loadParameterDouble(nh_private_,   std::string("max_vel"),          max_vel_);
        loadParameterIntArray(nh_private_, std::string("mapping/bl"),           patches_bl_);
        loadParameterIntArray(nh_private_, std::string("mapping/br"),           patches_br_);
        loadParameterIntArray(nh_private_, std::string("mapping/fl"),           patches_fl_);
        loadParameterIntArray(nh_private_, std::string("mapping/fr"),           patches_fr_);
        loadParameterIntArray(nh_private_, std::string("mapping/rb"),           patches_rb_);
        loadParameterIntArray(nh_private_, std::string("mapping/rf"),           patches_rf_);
        loadParameterIntArray(nh_private_, std::string("mapping/lb"),           patches_lb_);
        loadParameterIntArray(nh_private_, std::string("mapping/lf"),           patches_lf_);

        // Subscribe to /skin_data
        sub_skin_data_ = nh_.subscribe("skin_data", 1, &SkinSafeBase::skinNewMeasCallback, this);

        // Subscribe to /base_controller/command and publish to real controller (named unsafe)
        pub_cmd_vel_limits_ = nh_.advertise<safe_base_controller::TwistLimits>("/base_controller/command_limits", 1);
}

SkinSafeBase::~SkinSafeBase() {
}


void SkinSafeBase::skinNewMeasCallback(const boost::shared_ptr<skin_driver::skin_meas const>& msg) {
        // Only evaluate first 'num_sensors_' sensors, i.e. sensors 0 to num_sensors_-1
        sensor_data_.resize(num_sensors_);

        // Convert received data into sensor_data vector
        for (int i = 0; i < num_sensors_; i++){
                sensor_data_[i].id       = 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 < num_sensors_; i++)
//      {
//              ROS_INFO("    Sensor %i: x: alarm: %i", i+1, abs(sensor_data_[i].x - sensor_data_[i].ini));
//      }

        // Calculate if group of patches is blocked or not
        blocked_bl_ = checkPatchesBlocked(patches_bl_);
        blocked_br_ = checkPatchesBlocked(patches_br_);
        blocked_fl_ = checkPatchesBlocked(patches_fl_);
        blocked_fr_ = checkPatchesBlocked(patches_fr_);
        blocked_rb_ = checkPatchesBlocked(patches_rb_);
        blocked_rf_ = checkPatchesBlocked(patches_rf_);
        blocked_lb_ = checkPatchesBlocked(patches_lb_);
        blocked_lf_ = checkPatchesBlocked(patches_lf_);

        // Create TwistLimits and set to max_vel_ parameter
        safe_base_controller::TwistLimits vel_limits;
        vel_limits.linear.x_max = max_vel_;
        vel_limits.linear.x_min = -max_vel_;
        vel_limits.linear.y_max = max_vel_;
        vel_limits.linear.y_min = -max_vel_;
        vel_limits.angular.z_max = max_vel_;
        vel_limits.angular.z_min = -max_vel_;

        // Prohibit driving back
        if (blocked_bl_ || blocked_br_) {
                vel_limits.linear.x_min = 0.0;
                ROS_WARN("  -> back blocked");
        }

        // Prohibit driving fw
        if (blocked_fl_ || blocked_fr_) {
                vel_limits.linear.x_max = 0.0;
                ROS_WARN("  -> front blocked");
        }

        // Prohibit driving right
        if (blocked_lf_ || blocked_lb_) {
                vel_limits.linear.y_max = 0.0;
                ROS_WARN("  -> right blocked");
        }

        // Prohibit driving left
        if (blocked_rf_ || blocked_rb_) {
                vel_limits.linear.y_min = 0.0;
                ROS_WARN("  -> left blocked");
        }

        // Prohibit turning left
        if (blocked_rb_ || blocked_fr_ || blocked_lf_ || blocked_bl_) {
                vel_limits.angular.z_max = 0.0;
                ROS_WARN("  -> turn left blocked");
        }

        // Prohibit turning right
        if (blocked_rf_ || blocked_fl_ || blocked_lb_ || blocked_br_) {
                vel_limits.angular.z_min = 0.0;
                ROS_WARN("  -> turn right blocked");
        }

        pub_cmd_vel_limits_.publish(vel_limits);
}


bool SkinSafeBase::checkPatchesBlocked(std::vector<int> & patches)
{
        bool free = true;

        for (int i = 0; i < (int)patches.size(); i++)
        {
                int activation = (int)(abs(sensor_data_[patches[i]].x - sensor_data_[patches[i]].ini));
                if (activation < sensor_threshold_)
                {
                        ROS_INFO("  Sensor '%i' clear", patches[i]);
                }
                else
                {
                        free = false;
                        ROS_WARN("  Sensor '%i' blocked", patches[i]);
                }
        }

        return !free;
}


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

        // Create SkinSafeBase object
        SkinSafeBase skin_safe_base;

        // Spinning
        ros::spin();
}

// **********************************************************************
// ************ generic helper functions
// **********************************************************************


void SkinSafeBase::loadParameterDouble(ros::NodeHandle & nh_, std::string param, double & 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 SkinSafeBase::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 SkinSafeBase::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]));
        }
}

---

skin_safe_base
Author(s): Sebastian Haug
autogenerated on Fri Jan 11 09:50:57 2013