contact_sensor.h

Go to the documentation of this file.

/*******************************************************************/
/*    AUTHOR: Paal Arthur S. Thorseth                              */
/*    ORGN:   Dept of Eng Cybernetics, NTNU Trondheim              */
/*    FILE:   contact_sensor.h                                     */
/*    DATE:   May 19, 2021                                         */
/*                                                                 */
/* Copyright (C) 2021 Paal Arthur S. Thorseth,                     */
/*                    Adrian B. Ghansah                            */
/*                                                                 */
/* This program is free software: you can redistribute it          */
/* and/or modify it under the terms of the GNU General             */
/* Public License as published by the Free Software Foundation,    */
/* either version 3 of the License, or (at your option) any        */
/* later version.                                                  */
/*                                                                 */
/* This program is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty     */
/* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.         */
/* See the GNU General Public License for more details.            */
/*                                                                 */
/* You should have received a copy of the GNU General Public       */
/* License along with this program. If not, see                    */
/* <https://www.gnu.org/licenses/>.                                */
/*                                                                 */
/*******************************************************************/
 
#pragma once
 
// Gazebo
#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
 
// ROS
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "std_msgs/Int8MultiArray.h"
 
// Boost
#include <boost/algorithm/string.hpp>
 
class ContactSensor
{
    public: ContactSensor();
 
    public: virtual ~ContactSensor();
 
    public: void OnFlContactCallback(const ConstContactsPtr &_msg);
 
    public: void OnFrContactCallback(const ConstContactsPtr &_msg);
 
    public: void OnRlContactCallback(const ConstContactsPtr &_msg);
 
    public: void OnRrContactCallback(const ConstContactsPtr &_msg);
 
    public: void PublishQueueThread();
 
    protected: void InitGazebo();
 
    protected: void InitRos();
 
    protected: void InitRosQueueThreads();
 
    private: int footContacts[4];
 
    private: gazebo::transport::NodePtr gazeboNode;
 
    private: gazebo::transport::SubscriberPtr flContactSensorSub;
 
    private: gazebo::transport::SubscriberPtr frContactSensorSub;
 
    private: gazebo::transport::SubscriberPtr rlContactSensorSub;
 
    private: gazebo::transport::SubscriberPtr rrContactSensorSub;
 
    private: std::unique_ptr<ros::NodeHandle> rosNode;
 
    private: ros::Publisher contactStatePub;
 
    private: ros::CallbackQueue rosPublishQueue;
 
    private: std::thread rosPublishQueueThread;
};