iRobot Create 2 - Simulation

namespace gazebo
{
/**
 * Model plugin that consolidates bumper sensor states into a single create_msgs/Bumper message
*/
class CreateBumperModelRos : public ModelPlugin
{
public:
    CreateBumperModelRos();
    ~CreateBumperModelRos() = default;

    void Load(physics::ModelPtr parent, sdf::ElementPtr sdf);

private:
    /**
     * Contact sensor callback
    */
    void sensorCallback(const gazebo_msgs::ContactsStateConstPtr& msg);

    /**
     * Odom callback
    */
    void odomCallback(const nav_msgs::OdometryConstPtr& msg);

    /**
     * Light sensor callback
    */
    void lightSensorCallback(const std_msgs::UInt16ConstPtr& msg, uint16_t& field);

    /**
     * Update bumper publisher
    */
    void bumperPubTimerCallback(const ros::TimerEvent&);

    // Bumper message, publisher and timer
    ros::Publisher bumper_pub_;
    create_msgs::Bumper bumper_msg_;
    ros::Timer bumper_pub_timer_;
    // Subscriber for contact states
    ros::Subscriber contact_sub_;
    // Odom to get robot heading
    ros::Subscriber odom_sub_;

    // Contact parameters
    double front_contact_threshold_;

    // Robot heading
    tf::Vector3 heading_;
};
}

#include "create_gazebo/plugins/bumper_model_plugin.hpp"

#include "tf/tf.h"

namespace gazebo
{
// Register the plugin with Gazebo
GZ_REGISTER_MODEL_PLUGIN(CreateBumperModelRos)

CreateBumperModelRos::CreateBumperModelRos()
    : front_contact_threshold_{10.0} // Threshold for what is considered a bump at the front of the robot
    , heading_{1, 0, 0}              // The robot's current heading
{
}

void CreateBumperModelRos::Load(physics::ModelPtr, sdf::ElementPtr sdf)
{
    // Initialize ROS
    if (!ros::isInitialized())
    {
        int argc = 0;
        char** argv = nullptr;

        ros::init(argc, argv, "create_bumper_model_plugin");
    }

    // Load parameters
    double updateRate = 10.0;
    if (sdf->HasElement("updateRate"))
    {
        updateRate = sdf->Get<double>("updateRate");
    }

    std::string topic_name{"/bumper"};
    if (sdf->HasElement("topicName"))
    {
        topic_name = sdf->Get<std::string>("topicName");
    }

    std::string odom_topic{"/odom"};
    if (sdf->HasElement("odomTopic"))
    {
        odom_topic = sdf->Get<std::string>("odomTopic");
    }

    if (sdf->HasElement("frontContactThreshold"))
    {
        front_contact_threshold_ = sdf->Get<double>("frontContactThreshold");
    }

    // Setup publishers
    ros::NodeHandle nh;

    // The publisher for the bumper state
    bumper_pub_ = nh.advertise<create_msgs::Bumper>(topic_name, 1);
    // Timer that publishes the bumper message at {updateRate}
    bumper_pub_timer_ = nh.createTimer(ros::Duration{1.0 / updateRate}, 
                                       &CreateBumperModelRos::bumperPubTimerCallback, this);

    // Subscribe to the gazebo contact state message
    contact_sub_ = nh.subscribe("sim/bumper", 10, &CreateBumperModelRos::sensorCallback, this);

    // Odometry callback for getting the heading of the robot
    odom_sub_ = nh.subscribe(odom_topic, 10, &CreateBumperModelRos::odomCallback, this);
}

void CreateBumperModelRos::sensorCallback(const gazebo_msgs::ContactsStateConstPtr& msg)
{
    bumper_msg_.is_left_pressed = false;
    bumper_msg_.is_right_pressed = false;

    for (const auto& state : msg->states)
    {
        for (const auto& normal : state.contact_normals)
        {
            tf::Vector3 n;
            tf::vector3MsgToTF(normal, n);

            // Contact points are on a cylindrical collision body.
            // Therefore, the contact normals are all pointed inwards to the robot's center (base_link origin)

            // Invert the normal (point outwards from origin)
            n *= -1.0;

            tf::Vector3 x{1.0, 0.0, 0.0};
            // Check if the contact comes from the front of the robot
            if (heading_.dot(n) >= 0.0)
            {
                if (x.angle(n) <= tfRadians(front_contact_threshold_))
                {
                    bumper_msg_.is_left_pressed = true;
                    bumper_msg_.is_right_pressed = true;
                }
                else if (n.y() > 0.0)
                {
                    // left
                    bumper_msg_.is_left_pressed = true;
                    bumper_msg_.is_right_pressed = false;
                }
                else
                {
                    // right
                    bumper_msg_.is_left_pressed = false;
                    bumper_msg_.is_right_pressed = true;
                }
            }
        }
    }
}

void CreateBumperModelRos::odomCallback(const nav_msgs::OdometryConstPtr& msg)
{
    // Convert the orientation from odom into a tf type
    tf::Quaternion r{};
    tf::quaternionMsgToTF(msg->pose.pose.orientation, r);

    // Calculate the heading by rotating a X direction vector by the given quaternion
    const tf::Vector3 d{1, 0, 0};
    heading_ = tf::quatRotate(r, d);
}

void CreateBumperModelRos::bumperPubTimerCallback(const ros::TimerEvent&)
{
    // Update the bumper message
    bumper_pub_.publish(bumper_msg_);
}

}

namespace gazebo
{
/**
 * iRobot Create Cliff Sensor Plugin
*/
class CreateCliffRos : public RayPlugin
{
public:
    CreateCliffRos();
    ~CreateCliffRos() = default;

    /**
     * Load plugin
    */
    void Load(sensors::SensorPtr parent, sdf::ElementPtr sdf);

private:
    /**
     * Topic connect callback
    */
    void onConnect();
    /**
     * Topic disconnect callback
    */
    void onDisconnect();
    /**
     * Ray scan callback
    */
    void onScan(const ConstLaserScanStampedPtr& msg);

    // Parent sensor
    sensors::RaySensorPtr parent_;

    // Plugin parameters
    float min_range_;

    // ROS publisher
    PubMultiQueue pmq_;
    PubQueue<std_msgs::Bool>::Ptr pub_queue_;
    ros::Publisher pub_;

    // Gazebo subscriber
    transport::NodePtr node_;
    transport::SubscriberPtr scan_sub_;
};
}

#include "create_gazebo/plugins/cliff_sensor_plugin.hpp"

#include <gazebo_plugins/gazebo_ros_utils.h>


namespace gazebo
{
    GZ_REGISTER_SENSOR_PLUGIN(CreateCliffRos)

    CreateCliffRos::CreateCliffRos()
    {
    }

    void CreateCliffRos::Load(sensors::SensorPtr parent, sdf::ElementPtr sdf)
    {
        // Load the base plugin
        RayPlugin::Load(parent, sdf);

        parent_ = std::dynamic_pointer_cast<sensors::RaySensor>(parent);

        if (!parent_)
        {
            gzthrow("Parent sensor must be a Ray sensor");
        }

        // Load plugin parameters
        if (!sdf->HasElement("cliffRange"))
        {
            ROS_INFO_NAMED("cliff", "Cliff plugin missin parameter 'cliffRange', defaulting to 5cm");
            min_range_ = 0.05;
        }
        else
        {
            min_range_ = sdf->Get<double>("cliffRange");
        }

        std::string topic_name = "/cliff";
        if (sdf->HasElement("topicName"))
        {
            topic_name = sdf->Get<std::string>("topicName");
        }

        // Ensure ROS is up
        if (!ros::isInitialized())
        {
            ROS_FATAL_STREAM_NAMED("cliff", "ROS is not initialized, cannot load plugin");
            return;
        }

        // Get the world name and robot namespace
        // These will be used to subscribe to Gazebo laser scan messages (not ROS).
        const auto world_name = parent_->WorldName();
        const auto robot_namespace = GetRobotNamespace(parent_, sdf, "Cliff");

        // Gazebo node is used to subscribe to gazebo laser scan messages
        node_ = transport::NodePtr{new transport::Node{}};
        node_->Init(world_name);

        pmq_.startServiceThread();

        ros::NodeHandle nh{robot_namespace};

        ros::AdvertiseOptions opts = ros::AdvertiseOptions::create<std_msgs::Bool>(
            topic_name, 1,
            std::bind(&CreateCliffRos::onConnect, this),
            std::bind(&CreateCliffRos::onDisconnect, this),
            ros::VoidPtr{}, nullptr
        );

        pub_ = nh.advertise(opts);
        pub_queue_ = pmq_.addPub<std_msgs::Bool>();

        parent_->SetActive(false);

        ROS_INFO_STREAM_NAMED("cliff", "Starting cliff plugin: " << robot_namespace);
    }

    void CreateCliffRos::onConnect()
    {
        if (!scan_sub_)
        {
            // Only process scans in a subscriber is connected
            ROS_INFO_STREAM_NAMED("cliff", "Subscribing to scan messages");
            scan_sub_ = node_->Subscribe(parent_->Topic(), &CreateCliffRos::onScan, this);
        }
    }

    void CreateCliffRos::onDisconnect()
    {
        ROS_INFO_STREAM_NAMED("cliff", "Disconnecting from scan messages");
        scan_sub_.reset();
    }

    void CreateCliffRos::onScan(const ConstLaserScanStampedPtr& laser)
    {
        // If the detected distance is outside the minimum range, indicate that a cliff is detected
        std_msgs::Bool msg;
        msg.data = laser->scan().ranges(0) > min_range_;

        pub_queue_->push(msg, pub_);
    }
}

namespace gazebo
{
/**
 * Model plugin that consolidates individual cliff sensor sim topics into a single
 * /cliff topic that would be emitted by the ROS driver
*/
class CreateCliffModelRos : public ModelPlugin
{
public:
    CreateCliffModelRos();
    ~CreateCliffModelRos() = default;

    void Load(physics::ModelPtr parent, sdf::ElementPtr sdf);

private:
    /**
     * Cliff sensor callback
    */
    void sensorCallback(const std_msgs::BoolConstPtr& msg, bool& field);

    /**
     * 
    */
    void timerCallback(const ros::TimerEvent&);

    // Main cliff message publisher
    ros::Publisher cliff_pub_;
    // Cliff message
    create_msgs::Cliff cliff_msg_;
    // Publish timer
    ros::Timer pub_timer_;

    // Subscribers for individual sensors
    ros::Subscriber left_sub_;
    ros::Subscriber leftfront_sub_;
    ros::Subscriber right_sub_;
    ros::Subscriber rightfront_sub_;

    // Individual sensor states
    bool left_state_;
    bool leftfront_state_;
    bool right_state_;
    bool rightfront_state_;
};
}

#include "create_gazebo/plugins/cliff_model_plugin.hpp"

#include <functional>

namespace gazebo
{
// Register plugin with Gazebo
GZ_REGISTER_MODEL_PLUGIN(CreateCliffModelRos)

CreateCliffModelRos::CreateCliffModelRos()
{
}

void CreateCliffModelRos::Load(physics::ModelPtr, sdf::ElementPtr sdf)
{
    // Initialized ROS
    if (!ros::isInitialized())
    {
        int argc = 0;
        char** argv = nullptr;

        ros::init(argc, argv, "create_cliff_model_plugin");
    }

    // Load parameters
    double updateRate = 10.0;
    if (sdf->HasElement("updateRate"))
    {
        updateRate = sdf->Get<double>("updateRate");
    }

    std::string topic_name{"/cliff"};
    if (sdf->HasElement("topicName"))
    {
        topic_name = sdf->Get<std::string>("topicName");
    }

    ros::NodeHandle nh;

    // Setup publisher
    cliff_pub_ = nh.advertise<create_msgs::Cliff>(topic_name, 1);
    pub_timer_ = nh.createTimer(ros::Duration{1.0 / updateRate}, &CreateCliffModelRos::timerCallback, this);

    // Setup subscribers
    using namespace boost::placeholders;

    left_sub_ = nh.subscribe<std_msgs::Bool>("/sim/left_cliff", 1,
                             boost::bind(&CreateCliffModelRos::sensorCallback, this, _1, boost::ref(left_state_)));
    right_sub_ = nh.subscribe<std_msgs::Bool>("/sim/right_cliff", 1,
                             boost::bind(&CreateCliffModelRos::sensorCallback, this, _1, boost::ref(right_state_)));
    leftfront_sub_ = nh.subscribe<std_msgs::Bool>("/sim/leftfront_cliff", 1,
                             boost::bind(&CreateCliffModelRos::sensorCallback, this, _1, boost::ref(leftfront_state_)));
    rightfront_sub_ = nh.subscribe<std_msgs::Bool>("/sim/rightfront_cliff", 1,
                             boost::bind(&CreateCliffModelRos::sensorCallback, this, _1, boost::ref(rightfront_state_)));
}

void CreateCliffModelRos::sensorCallback(const std_msgs::BoolConstPtr& msg, bool& field)
{
    field = msg->data;
}

void CreateCliffModelRos::timerCallback(const ros::TimerEvent&)
{
    cliff_msg_.header.stamp = ros::Time::now();
    cliff_msg_.header.seq++;

    cliff_msg_.is_cliff_left = left_state_;
    cliff_msg_.is_cliff_front_left = leftfront_state_;
    cliff_msg_.is_cliff_right = right_state_;
    cliff_msg_.is_cliff_front_right = rightfront_state_;

    cliff_pub_.publish(cliff_msg_);
}

}

static constexpr uint16_t MAX_LIGHT_VALUE = 4096;
// ...
void CreateLightRos::onScan(const ConstLaserScanStampedPtr& laser)
{
    std_msgs::UInt16 msg;

    const auto distance = laser->scan().ranges(0);

    // Check if there is no obstacle within the sensor range
    if (std::isinf(distance))
    {
        msg.data = 0;
    }
    else
    {
        const auto ratio = distance / laser->scan().range_max();
        msg.data = MAX_LIGHT_VALUE * (1.0 - ratio);
    }

    pub_queue_->push(msg, pub_);
}

void CreateBumperModelRos::Load(physics::ModelPtr, sdf::ElementPtr sdf)
{
    // ...

    light_subs_[0] = nh.subscribe<std_msgs::UInt16>("/sim/light_left", 1,
                                                    boost::bind(&CreateBumperModelRos::lightSensorCallback, this,
                                                    _1, boost::ref(bumper_msg_.light_signal_left)));
    light_subs_[1] = nh.subscribe<std_msgs::UInt16>("/sim/light_frontleft", 1,
                                                    boost::bind(&CreateBumperModelRos::lightSensorCallback, this,
                                                    _1, boost::ref(bumper_msg_.light_signal_front_left)));
    light_subs_[2] = nh.subscribe<std_msgs::UInt16>("/sim/light_centerleft", 1,
                                                    boost::bind(&CreateBumperModelRos::lightSensorCallback, this,
                                                    _1, boost::ref(bumper_msg_.light_signal_center_left)));
    light_subs_[3] = nh.subscribe<std_msgs::UInt16>("/sim/light_centerright", 1,
                                                    boost::bind(&CreateBumperModelRos::lightSensorCallback, this,
                                                    _1, boost::ref(bumper_msg_.light_signal_center_right)));
    light_subs_[4] = nh.subscribe<std_msgs::UInt16>("/sim/light_frontright", 1,
                                                    boost::bind(&CreateBumperModelRos::lightSensorCallback, this,
                                                    _1, boost::ref(bumper_msg_.light_signal_front_right)));
    light_subs_[5] = nh.subscribe<std_msgs::UInt16>("/sim/light_right", 1,
                                                    boost::bind(&CreateBumperModelRos::lightSensorCallback, this,
                                                    _1, boost::ref(bumper_msg_.light_signal_right)));
}

// ...

void CreateBumperModelRos::lightSensorCallback(const std_msgs::UInt16ConstPtr& msg, uint16_t& field)
{
    field = msg->data;
}