Stopping a mobile robot

edit

I tried to modify code given in tutorial http://gazebosim.org/wiki/Tutorials/1...robot/mobilebase_laser to stop it .When there is no obstacle .I set the force to 0.0 even it moving continuously .I checked it is going in the condition and setting force equal to zero by using printf .I am the beginner in gazebo.I am pasting code and highlighting modified portion.Please suggest how to stop robot if there is any other way .

namespace gazebo {
class MobileBasePlugin : public ModelPlugin { public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {

  // Store the pointer to the model
  this->model = _parent;

  // Load parameters for this plugin
  if (this->LoadParams(_sdf))
  {
    // testing to see if race condition exists
    gzerr << this->leftWheelJoint->GetAngle(0) << "\n";
    gzerr << this->rightWheelJoint->GetAngle(0) << "\n";
    // Listen to the update event. This event is broadcast every
    // simulation iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateStart(
        boost::bind(&MobileBasePlugin::OnUpdate, this));
  }
}

public: bool LoadParams(sdf::ElementPtr _sdf) 
{

  // Find controller gain
  if (!_sdf->HasElement("gain"))
  {
    gzerr << "param [gain] not found\n";
    return false;
  }
  else
  {
    // Get sensor name
    this->gain =
      _sdf->GetElement("gain")->GetValueDouble();
  }

  // Find sensor name from plugin param
  if (!_sdf->HasElement("ray_sensor"))
  {
    gzerr << "param [ray_sensor] not found\n";
    return false;
  }
  else
  {
    // Get sensor name
    std::string sensorName =
      _sdf->GetElement("ray_sensor")->GetValueString();

    // Get pointer to sensor using the SensorMangaer
    sensors::SensorPtr sensor =
      sensors::SensorManager::Instance()->GetSensor(sensorName);

    if (!sensor)
    {
      gzerr << "sensor by name ["
            << sensorName
            << "] not found in model\n";
      return false;
    }

    this->laser = boost::shared_dynamic_cast<sensors::RaySensor>
      (sensor);
    if (!this->laser)
    {
      gzerr << "laser by name ["
            << sensorName
            << "] not found in model\n";
      return false;
    }
  }

  // Load joints from plugin param
  if (!this->FindJointByParam(_sdf, this->leftWheelJoint,
                         "left_wheel_hinge") ||
      !this->FindJointByParam(_sdf, this->rightWheelJoint,
                         "right_wheel_hinge"))
    return false;

  // success
  return true;
}

public: bool FindJointByParam(sdf::ElementPtr _sdf,
                              physics::JointPtr &_joint,
                              std::string _param)
{
  if (!_sdf->HasElement(_param))
  {
    gzerr << "param [" << _param << "] not found\n";
    return false;
  }
  else
  {
    _joint = this->model->GetJoint(
      _sdf->GetElement(_param)->GetValueString());

    if (!_joint)
    {
      gzerr << "joint by name ["
            << _sdf->GetElement(_param)->GetValueString()
            << "] not found in model\n";
      return false;
    }
  }
  return true;
}

// Called by the world update start event
public: void OnUpdate()
{
  unsigned int n = this->laser->GetRangeCount();

  double min_dist = 1e6;
  for (unsigned int i = 0; i < n; ++i)
  {

    if (this->laser->GetRange(i) < min_dist)
    {
      min_dist = this->laser->GetRange(i);


}
  }

  double target_dist = 2.0;

strong text if (min_dist < this->laser->GetRangeMax()) {

printf("inside if  \n");
    double torque = this->gain *( (min_dist - target_dist)*0.1 );


    this->leftWheelJoint->SetForce(0, torque);
    this->rightWheelJoint->SetForce(0, torque);
  } 

else 
   {
     printf("inside else condition \n");
         this->leftWheelJoint->SetForce(0,0.0);
         this->rightWheelJoint->SetForce(0,0.0);
    }

strong text }

// Pointer to the model
private: physics::ModelPtr model;

// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;

private: physics::JointPtr leftWheelJoint;
private: physics::JointPtr rightWheelJoint;
private: sensors::RaySensorPtr laser;
private: double gain;

};

// Register this plugin with the simulator GZREGISTERMODEL_PLUGIN(MobileBasePlugin) }