text: /*
  * Copyright (C) 2017 Vrije Universiteit Amsterdam
  *
  * Licensed under the Apache License, Version 2.0 (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.
  *
  * Description: TODO: <Add brief description about file purpose>
  * Author: Elte Hupkes
  * Date: Mar 27, 2015
  *
  */
  
  #include <string>
  
  #include <revolve/gazebo/sensors/TouchSensor.h>
  
  namespace gz = gazebo;
  
  using namespace revolve::gazebo;
  
  /////////////////////////////////////////////////
  TouchSensor::TouchSensor(
      ::gazebo::physics::ModelPtr _model,
      sdf::ElementPtr _sensor,
      std::string _partId,
      std::string _sensorId)
      : Sensor(_model, _sensor, _partId, _sensorId, 1)
      , lastValue_(false)
  {
    this->castSensor_ = std::dynamic_pointer_cast< gz::sensors::ContactSensor >(
        this->sensor_);
  
    if (not this->castSensor_)
    {
      std::cerr << "Creating a touch sensor with a non-contact sensor object"
                << std::endl;
      throw std::runtime_error("Sensor error");
    }
  
    // Sensor must always update
    this->castSensor_->SetActive(true);
  
    // Add update connection that will produce new value
    this->updateConnection_ = this->sensor_->ConnectUpdated(
        std::bind(&TouchSensor::OnUpdate, this));
  }
  
  /////////////////////////////////////////////////
  TouchSensor::~TouchSensor() = default;
  
  /////////////////////////////////////////////////
  void TouchSensor::OnUpdate()
  {
    auto contacts = this->castSensor_->Contacts();
    this->lastValue_ = contacts.contact_size() > 0;
  }
  
  /////////////////////////////////////////////////
  void TouchSensor::Read(double *_input)
  {
    _input[0] = this->lastValue_ ? 1 : 0;
  }
  