////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Get the world name.
  this->world_ = _model->GetWorld();
  
  // load parameters
  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if (!_sdf->HasElement("bodyName"))
  {
    ROS_FATAL("force plugin missing <bodyName>, cannot proceed");
    return;
  }
  else
    this->link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();

  this->link_ = _model->GetLink(this->link_name_);
  if (!this->link_)
  {
    ROS_FATAL("gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str());
    return;
  }

  if (!_sdf->HasElement("topicName"))
  {
    ROS_FATAL("force plugin missing <topicName>, cannot proceed");
    return;
  }
  else
    this->topic_name_ = _sdf->GetElement("topicName")->Get<std::string>();


  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
      << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);

  // Custom Callback Queue
  ros::SubscribeOptions so = ros::SubscribeOptions::create<geometry_msgs::Wrench>(
    this->topic_name_,1,
    boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1),
    ros::VoidPtr(), &this->queue_);
  this->sub_ = this->rosnode_->subscribe(so);

  // Custom Callback Queue
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) );

  // New Mechanism for Updating every World Cycle
  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRosForce::UpdateChild, this));
}
void BatteryConsumer::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    if (!ros::isInitialized())
    {
        ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                         << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
        return;
    }
    double default_consume = DEFAULT_CONSUMER_CONSUME;
    std::string name = DEFAULT_CONSUMER_NAME;
    if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
    if (_sdf->HasElement("default_consume")) default_consume = _sdf->GetElement("default_consume")->Get<double>();
    if (_sdf->HasElement("default_consumer_name")) name = _sdf->GetElement("default_consumer_name")->Get<std::string>();


    default_consumer.first = name;
    default_consumer.second = default_consume;
    consumers.insert(default_consumer);


    this->model = _parent;
    n = ros::NodeHandle(namespace_);


    battery_pub = n.advertise<std_msgs::Float64>("battery_state", 1);

    add_consumer_service = n.advertiseService("add_consumer",&BatteryConsumer::addConsumer,this);


    remove_consumer_service = n.advertiseService("remove_consumer",&BatteryConsumer::removeConsumer,this);
    last_time = model->GetWorld()->GetSimTime().Double();
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                                 boost::bind(&BatteryConsumer::OnUpdate, this, _1));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboTreasure::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  model_ = _model;
  world_ = _model->GetWorld();
  link_ = _model->GetLink();
  link_name_ = link_->GetName();
  namespace_.clear();

  vel_x = vel_y = vel_yaw = 0;
  static_object_ = false;
  last_time_ = world_->GetSimTime();
  terminated_ = false;

  // load parameters from sdf
  if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();

  if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue())
    {
      link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
      link_ = _model->GetLink(link_name_);
    }

  if (_sdf->HasElement("staticObject") && _sdf->GetElement("staticObject")->GetValue())
    {
      static_object_ = _sdf->GetElement("staticObject")->Get<std::string>() == "true"?true:false;
    }

  // boost::random::mt19937 rng;
  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> x_rand(-40, 40);
  boost::random::uniform_int_distribution<> y_rand(-20, 20);
  double x = x_rand(rng);
  double y = y_rand(rng);
  model_->SetLinkWorldPose(math::Pose(x, y, 0.2, 0, 0, 0), link_);

  if (!link)
  {
    ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }


  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                     << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true);  // set latch true
  ros::NodeHandle param_handle(*node_handle_, "controller");



  update_connection_ = event::Events::ConnectWorldUpdateBegin(
                                                              boost::bind(&GazeboTreasure::Update, this));
}
void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) {
    // Store the pointer to the model
    this->parent_ = _parent;
    this->world_ = _parent->GetWorld();

    this->robot_namespace_ = parent_->GetName ();
    if ( !_sdf->HasElement ( "robotNamespace" ) ) {
        ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin missing <robotNamespace>, defaults to \"%s\"",
                   this->robot_namespace_.c_str() );
    } else {
        this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get<std::string>();
        if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName ();
    }
    if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/";
    rosnode_ = boost::shared_ptr<ros::NodeHandle> ( new ros::NodeHandle ( this->robot_namespace_ ) );

    if ( !_sdf->HasElement ( "jointName" ) ) {
        ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" );
    } else {
        sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ;
        std::string joint_names = element->Get<std::string>();
        boost::erase_all ( joint_names, " " );
        boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) );
    }

    this->update_rate_ = 100.0;
    if ( !_sdf->HasElement ( "updateRate" ) ) {
        ROS_WARN_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin (ns = %s) missing <updateRate>, defaults to %f",
                   this->robot_namespace_.c_str(), this->update_rate_ );
    } else {
        this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get<double>();
    }

    // Initialize update rate stuff
    if ( this->update_rate_ > 0.0 ) {
        this->update_period_ = 1.0 / this->update_rate_;
    } else {
        this->update_period_ = 0.0;
    }
    last_update_time_ = this->world_->GetSimTime();

    for ( unsigned int i = 0; i< joint_names_.size(); i++ ) {
        joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) );
        ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() );
    }

    ROS_INFO_NAMED("joint_state_publisher", "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() );

    tf_prefix_ = tf::getPrefixParam ( *rosnode_ );
    joint_state_publisher_ = rosnode_->advertise<sensor_msgs::JointState> ( "joint_states",1000 );

    last_update_time_ = this->world_->GetSimTime();
    // Listen to the update event. This event is broadcast every
    // simulation iteration.
    this->updateConnection = event::Events::ConnectWorldUpdateBegin (
                                 boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) );
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboPanel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  gzwarn << "The GazeboPanel plugin is DEPRECATED in ROS hydro." << std::endl;

  model_ = _model;
  world_ = _model->GetWorld();
  link_ = _model->GetLink();
  link_name_ = link_->GetName();
  namespace_.clear();

  terminated_ = false;

  // load parameters from sdf
  if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();

  if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue())
    {
      link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
      link_ = _model->GetLink(link_name_);
    }

  if (_sdf->HasElement("jointName") && _sdf->GetElement("jointName")->GetValue())
    {
      std::string joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
      joint_ = _model->GetJoint(joint_name_);
    }
  else
    {
      joint_ = _model->GetJoint("stem_joint");
    }

  if (!link)
  {
    ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
    return;
  }


  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                     << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  pub_score_ = node_handle_->advertise<std_msgs::String>("score", 1, true);  // set latch true
  pub_time_ = node_handle_->advertise<std_msgs::String>("remaining_time", 1);
  ros::NodeHandle param_handle(*node_handle_, "controller");



  update_connection_ = event::Events::ConnectWorldUpdateBegin(
                                                              boost::bind(&GazeboPanel::Update, this));
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
  // Get then name of the parent sensor
  sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(_sensor);
  if (!sensor_)
  {
    gzthrow("GazeboRosSonar requires a Ray Sensor as its parent");
    return;
  }

  // Get the world name.
  std::string worldName = sensor_->GetWorldName();
  world = physics::get_world(worldName);

  // load parameters
  if (!_sdf->HasElement("robotNamespace"))
    namespace_.clear();
  else
    namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";

  if (!_sdf->HasElement("frameId"))
    frame_id_ = "";
  else
    frame_id_ = _sdf->GetElement("frameId")->GetValueString();

  if (!_sdf->HasElement("topicName"))
    topic_ = "sonar";
  else
    topic_ = _sdf->GetElement("topicName")->GetValueString();

  sensor_model_.Load(_sdf);

  range_.header.frame_id = frame_id_;
  range_.radiation_type = sensor_msgs::Range::ULTRASOUND;
  range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).GetAsRadian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).GetAsRadian()));
  range_.max_range = sensor_->GetRangeMax();
  range_.min_range = sensor_->GetRangeMin();

  // start ros node
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  node_handle_ = new ros::NodeHandle(namespace_);
  publisher_ = node_handle_->advertise<sensor_msgs::Range>(topic_, 1);

  Reset();
  updateConnection = sensor_->GetLaserShape()->ConnectNewLaserScans(
        boost::bind(&GazeboRosSonar::Update, this));

  // activate RaySensor
  sensor_->SetActive(true);
}
void GazeboAltimeterPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  // Configure Gazebo Integration
  model_ = _model;
  world_ = model_->GetWorld();
  namespace_.clear();
  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzerr << "[gazebo_altimeter_plugin] Please specify a robotNamespace.\n";
  if (_sdf->HasElement("linkName"))
    link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
  else
    gzerr << "[gazebo_altimeter_plugin] Please specify a linkName.\n";
  link_ = model_->GetLink(link_name_);
  if (link_ == NULL)
    gzthrow("[gazebo_altimeter_plugin] Couldn't find specified link \"" << link_name_ << "\".");
  // Connect to the Gazebo Update
  this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAltimeterPlugin::OnUpdate, this, _1));
  frame_id_ = link_name_;

  // load params from xacro
  getSdfParam<std::string>(_sdf, "altimeterTopic", alt_topic_, "altimeter");
  getSdfParam<double>(_sdf, "altimeterMinRange", min_range_, 0.381);
  getSdfParam<double>(_sdf, "altimeterMaxRange", max_range_, 6.45);
  getSdfParam<double>(_sdf, "altimeterErrorStdev", error_stdev_, 0.10);
  getSdfParam<double>(_sdf, "altimeterFOV", field_of_view_, 1.107);
  getSdfParam<double>(_sdf, "altimeterPublishRate", pub_rate_, 20.0);
  getSdfParam<bool>(_sdf, "altimeterNoiseOn", alt_noise_on_, true);
  getSdfParam<bool>(_sdf, "publishFloat32", publish_float_, false);
  last_time_ = world_->GetSimTime();

  // Configure ROS Integration
  node_handle_ = new ros::NodeHandle(namespace_);
  if(publish_float_)
      alt_pub_ = node_handle_->advertise<std_msgs::Float32>(alt_topic_, 10);
  else
      alt_pub_ = node_handle_->advertise<sensor_msgs::Range>(alt_topic_, 10);

  // Fill static members of alt message.
  alt_message_.header.frame_id = frame_id_;
  alt_message_.max_range = max_range_;
  alt_message_.min_range = min_range_;
  alt_message_.field_of_view = field_of_view_;
  alt_message_.radiation_type = sensor_msgs::Range::ULTRASOUND;

  // Configure Noise
  random_generator_= std::default_random_engine(std::chrono::system_clock::now().time_since_epoch().count());
  standard_normal_distribution_ = std::normal_distribution<double>(0.0, error_stdev_);
}
Esempio n. 8
0
gz::common::PID Motor::createPid(sdf::ElementPtr pidElem) {
	double pv = 0, iv = 0, dv = 0, iMax = 0, iMin = 0,
			cmdMax = 0, cmdMin = 0;

	if (pidElem->HasElement("rv:p")) pv = pidElem->GetElement("rv:p")->Get<double>();
	if (pidElem->HasElement("rv:i")) iv = pidElem->GetElement("rv:i")->Get<double>();
	if (pidElem->HasElement("rv:d")) dv = pidElem->GetElement("rv:d")->Get<double>();
	if (pidElem->HasElement("rv:i_max")) iMax = pidElem->GetElement("rv:i_max")->Get<double>();
	if (pidElem->HasElement("rv:i_min")) iMin = pidElem->GetElement("rv:i_min")->Get<double>();
	if (pidElem->HasElement("rv:cmd_max")) cmdMax = pidElem->GetElement("rv:cmd_max")->Get<double>();
	if (pidElem->HasElement("rv:cmd_min")) cmdMin = pidElem->GetElement("rv:cmd_min")->Get<double>();

	return gz::common::PID(pv, iv, dv, iMax, iMin, cmdMax, cmdMin);
}
	bool AllWheelSteeringPlugin::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;
	}
    void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
        // Store the pointer to the model.
        model_ = _model;
        world_ = model_->GetWorld();

        double wind_gust_start = kDefaultWindGustStart;
        double wind_gust_duration = kDefaultWindGustDuration;

        if (_sdf->HasElement("robotNamespace"))
            namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
        else
            gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n";
        node_handle_ = new ros::NodeHandle(namespace_);

        if (_sdf->HasElement("xyzOffset"))
            xyz_offset_ = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();
        else
            gzerr << "[gazebo_wind_plugin] Please specify a xyzOffset.\n";

        getSdfParam<std::string>(_sdf, "windPubTopic", wind_pub_topic_, "/" + namespace_ + wind_pub_topic_);
        getSdfParam<std::string>(_sdf, "frameId", frame_id_, frame_id_);
        getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_);
        // Get the wind params from SDF.
        getSdfParam<double>(_sdf, "windForceMean", wind_force_mean_, wind_force_mean_);
        getSdfParam<double>(_sdf, "windForceVariance", wind_force_variance_, wind_force_variance_);
        getSdfParam<math::Vector3>(_sdf, "windDirection", wind_direction_, wind_direction_);
        // Get the wind gust params from SDF.
        getSdfParam<double>(_sdf, "windGustStart", wind_gust_start, wind_gust_start);
        getSdfParam<double>(_sdf, "windGustDuration", wind_gust_duration, wind_gust_duration);
        getSdfParam<double>(_sdf, "windGustForceMean", wind_gust_force_mean_, wind_gust_force_mean_);
        getSdfParam<double>(_sdf, "windGustForceVariance", wind_gust_force_variance_, wind_gust_force_variance_);
        getSdfParam<math::Vector3>(_sdf, "windGustDirection", wind_gust_direction_, wind_gust_direction_);

        wind_direction_.Normalize();
        wind_gust_direction_.Normalize();
        wind_gust_start_ = common::Time(wind_gust_start);
        wind_gust_end_ = common::Time(wind_gust_start + wind_gust_duration);

        link_ = model_->GetLink(link_name_);
        if (link_ == NULL) gzthrow("[gazebo_wind_plugin] Couldn't find specified link \"" << link_name_ << "\".");


        // Listen to the update event. This event is broadcast every
        // simulation iteration.
        update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboWindPlugin::OnUpdate, this, _1));

        wind_pub_ = node_handle_->advertise<geometry_msgs::WrenchStamped>(wind_pub_topic_, 10);
    }
void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix)
{
  gain_p = 0.0;
  gain_d = 0.0;
  gain_i = 0.0;
  time_constant = 0.0;
  limit = -1.0;

  if (!_sdf) return;
  // _sdf->PrintDescription(_sdf->GetName());
  if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->GetValueDouble();
  if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->GetValueDouble();
  if (_sdf->HasElement(prefix + "IntegralGain"))     gain_i = _sdf->GetElement(prefix + "IntegralGain")->GetValueDouble();
  if (_sdf->HasElement(prefix + "TimeConstant"))     time_constant = _sdf->GetElement(prefix + "TimeConstant")->GetValueDouble();
  if (_sdf->HasElement(prefix + "Limit"))            limit = _sdf->GetElement(prefix + "Limit")->GetValueDouble();
}
void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
  // Get then name of the parent sensor
  this->parentSensor =
    boost::dynamic_pointer_cast<sensors::RaySensor>(_parent);

  if (!this->parentSensor)
    gzthrow("RayPlugin requires a Ray Sensor as its parent");

  this->world = physics::get_world(this->parentSensor->GetWorldName());

  this->newLaserScansConnection =
    this->parentSensor->GetLaserShape()->ConnectNewLaserScans(
      boost::bind(&RayPlugin::OnNewLaserScans, this));

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);

  lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>("lidar", 10);
}
void GazeboLidarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
  if(kPrintOnPluginLoad) {
    gzdbg << __FUNCTION__ << "() called." << std::endl;
  }

  // Get then name of the parent sensor
  this->parentSensor =
    std::dynamic_pointer_cast<sensors::RaySensor>(_parent);

  if (!this->parentSensor)
    gzthrow("RayPlugin requires a Ray Sensor as its parent");

  this->world = physics::get_world(this->parentSensor->WorldName());

  this->newLaserScansConnection =
    this->parentSensor->LaserShape()->ConnectNewLaserScans(
      boost::bind(&GazeboLidarPlugin::OnNewLaserScans, this));

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);

  const string scopedName = _parent->ParentName();

  string topicName = "~/" + scopedName + "/lidar";
  boost::replace_all(topicName, "::", "/");

  lidar_pub_ = node_handle_->Advertise<lidar_msgs::msgs::lidar>(topicName, 10);
}
Esempio n. 14
0
PositionMotor::PositionMotor(gz::physics::ModelPtr model, std::string partId,
							 std::string motorId, sdf::ElementPtr motor):
	JointMotor(model, partId, motorId, motor, 1),
	positionTarget_(0),
	noise_(0) {
	// Retrieve upper / lower limit from joint set in parent constructor
	// Truncate ranges to [-pi, pi]
	upperLimit_ = fmin(M_PI, joint_->GetUpperLimit(0).Radian());
	lowerLimit_ = fmax(-M_PI, joint_->GetLowerLimit(0).Radian());
	fullRange_ = (upperLimit_ - lowerLimit_ + 1e-12) >= (2 * M_PI);

	if (motor->HasElement("rv:pid")) {
		auto pidElem = motor->GetElement("rv:pid");
		pid_ = Motor::createPid(pidElem);
	}

	auto noiseParam = motor->GetAttribute("noise");
	if (noiseParam) {
		noiseParam->Get(noise_);
	}

	// I've asked this question at the Gazebo forums:
	// http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/
	// Until it is answered I'm resorting to calling ODE functions directly to get this
	// to work. This will result in some deprecation warnings.
	// It has the added benefit of not requiring the world update connection though.
//	updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin(boost::bind(
//			&PositionMotor::OnUpdate, this, _1));

	double maxEffort = joint_->GetEffortLimit(0);
	joint_->SetParam("fmax", 0, maxEffort);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
    DepthCameraPlugin::Load(_parent, _sdf);

    // copying from DepthCameraPlugin into GazeboRosCameraUtils
    this->parentSensor_ = this->parentSensor;
    this->width_ = this->width;
    this->height_ = this->height;
    this->depth_ = this->depth;
    this->format_ = this->format;
    this->camera_ = this->depthCamera;

    // using a different default
    if (!_sdf->HasElement("imageTopicName"))
        this->image_topic_name_ = "ir/image_raw";
    if (!_sdf->HasElement("cameraInfoTopicName"))
        this->camera_info_topic_name_ = "ir/camera_info";

    // point cloud stuff
    if (!_sdf->HasElement("pointCloudTopicName"))
        this->point_cloud_topic_name_ = "points";
    else
        this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get<std::string>();

    // depth image stuff
    if (!_sdf->HasElement("depthImageTopicName"))
        this->depth_image_topic_name_ = "depth/image_raw";
    else
        this->depth_image_topic_name_ = _sdf->GetElement("depthImageTopicName")->Get<std::string>();

    if (!_sdf->HasElement("depthImageCameraInfoTopicName"))
        this->depth_image_camera_info_topic_name_ = "depth/camera_info";
    else
        this->depth_image_camera_info_topic_name_ = _sdf->GetElement("depthImageCameraInfoTopicName")->Get<std::string>();

    if (!_sdf->HasElement("pointCloudCutoff"))
        this->point_cloud_cutoff_ = 0.4;
    else
        this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<double>();
    if (!_sdf->HasElement("pointCloudCutoffMax"))
        this->point_cloud_cutoff_max_ = 5.0;
    else
        this->point_cloud_cutoff_max_ = _sdf->GetElement("pointCloudCutoffMax")->Get<double>();

    load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosOpenniKinect::Advertise, this));
    GazeboRosCameraUtils::Load(_parent, _sdf);
}
void GazeboAirspeedPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  // Store the pointer to the model
  model_ = _model;
  world_ = model_->GetWorld();

  // default params
  namespace_.clear();

  if (_sdf->HasElement("namespace"))
    namespace_ = _sdf->GetElement("namespace")->Get<std::string>();
  else
    gzerr << "[gazebo_imu_plugin] Please specify a namespace.\n";
  nh_ = new ros::NodeHandle(namespace_);

  if (_sdf->HasElement("linkName"))
    link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
  else
    gzerr << "[gazebo_imu_plugin] Please specify a linkName.\n";
  link_ = model_->GetLink(link_name_);
  if (link_ == NULL)
    gzthrow("[gazebo_imu_plugin] Couldn't find specified link \"" << link_name_ << "\".");

  frame_id_ = link_name_;

  getSdfParam<std::string>(_sdf, "airspeedTopic", airspeed_topic_, "airspeed/data");
  getSdfParam<double>(_sdf, "pressureBias", pressure_bias_, 0);
  getSdfParam<double>(_sdf, "pressureNoiseSigma", pressure_noise_sigma_,10);
  getSdfParam<double>(_sdf, "airDensity", rho_, 1.225);
  getSdfParam<double>(_sdf, "maxPressure", max_pressure_, 4000.0);
  getSdfParam<double>(_sdf, "minPressure", min_pressure_, 0.0);


  last_time_ = world_->GetSimTime();

  // Listen to the update event. This event is broadcast every simulation iteration.
  this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboAirspeedPlugin::OnUpdate, this, _1));

  airspeed_pub_ = nh_->advertise<sensor_msgs::FluidPressure>(airspeed_topic_, 10);

  // Fill static members of airspeed message.
  airspeed_message_.header.frame_id = frame_id_;
  airspeed_message_.variance = pressure_noise_sigma_*pressure_noise_sigma_;

  standard_normal_distribution_ = std::normal_distribution<double>(0.0, 1.0);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosOpenniKinect::Load(sensors::SensorPtr _parent,
		sdf::ElementPtr _sdf) {

	std::cout << "LOAD OPENNI" << std::endl;
	DepthCameraPlugin::Load(_parent, _sdf);

	// copying from DepthCameraPlugin into GazeboRosCameraUtils
	this->parentSensor_ = this->parentSensor;
	this->width_ = this->width;
	this->height_ = this->height;
	this->depth_ = this->depth;
	this->format_ = this->format;
	this->camera_ = this->depthCamera;

	// using a different default
	if (!_sdf->GetElement("imageTopicName"))
		this->image_topic_name_ = "ir/image_raw";
	if (!_sdf->HasElement("cameraInfoTopicName"))
		this->camera_info_topic_name_ = "ir/camera_info";

	// point cloud stuff
	if (!_sdf->GetElement("pointCloudTopicName"))
		this->point_cloud_topic_name_ = "points";
	else
		this->point_cloud_topic_name_ =
				_sdf->GetElement("pointCloudTopicName")->Get<std::string>();

	// depth image stuff
	this->depth_image_topic_name_ = "mukodik";
//	if (!_sdf->GetElement("depthImageTopicName"))
//		this->depth_image_topic_name_ = "depth/image_raw";
//	else
//		this->depth_image_topic_name_ =
//				_sdf->GetElement("depthImageTopicName")->Get<std::string>();

	if (!_sdf->GetElement("depthImageCameraInfoTopicName"))
		this->depth_image_camera_info_topic_name_ = "depth/camera_info";
	else
		this->depth_image_camera_info_topic_name_ = _sdf->GetElement(
				"depthImageCameraInfoTopicName")->Get<std::string>();

	if (!_sdf->GetElement("pointCloudCutoff"))
		this->point_cloud_cutoff_ = 0.4;
	else
		this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get<
				double>();

	load_connection_ = GazeboRosCameraUtils::OnLoad(
			boost::bind(&GazeboRosOpenniKinect::Advertise, this));

	std::cout << "Establishing Update Connection!" << std::endl;

	update_connection_ = event::Events::ConnectPreRender(
			boost::bind(&GazeboRosOpenniKinect::OnUpdate, this));

	GazeboRosCameraUtils::Load(_parent, _sdf);
}
Esempio n. 18
0
void MotorSchemaArch::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    // Store the pointer to the model
    this->_model = _parent;

    // Load Parameters
    if( this->initialize("MotorSchema", _parent, _sdf) )
    {
        if( _sdf->HasElement("gain_open") )
        {
            gzmsg << "Open Gain: " << _sdf->GetElement("gain_open")->Get<double>() << endl;
            this->kOpen = _sdf->GetElement("gain_open")->Get<double>();
        }
        else
        {
            gzwarn << "No gain_open parameter defined, defaulting to 1.0d" << endl;
            this->kOpen = 1.0d;
        }
        // Listen to the update event every iteration
        this->_updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&MotorSchemaArch::OnUpdate, this, _1));
    }
}
Esempio n. 19
0
/** on loading of the plugin
 * @param _parent Parent Model
 */
void Puck::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  printf("Loading Puck Plugin of model %s\n", _parent->GetName().c_str());
  // Store the pointer to the model
  this->model_ = _parent;  

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Puck::OnUpdate, this, _1));

  // Create the communication Node for communication with fawkes
  this->node_ = transport::NodePtr(new transport::Node());
  // the namespace is set to the world name!
  this->node_->Init(model_->GetWorld()->GetName());

  // register visual publisher
  this->visual_pub_ = this->node_->Advertise<msgs::Visual>("~/visual");
  
  // initialize without rings or cap
  this->ring_count_ = 0;
  this->have_cap = false;
  this->announced_ = false;
  
  this->new_puck_publisher = this->node_->Advertise<gazsim_msgs::NewPuck>("~/new_puck");
  
  // subscribe for puck commands
  this->command_subscriber = this->node_->Subscribe(std::string("~/pucks/cmd"), &Puck::on_command_msg, this);
  
  // publisher for workpiece command results
  this->workpiece_result_pub_ = node_->Advertise<gazsim_msgs::WorkpieceResult>("~/pucks/cmd/result");
  
  if (!_sdf->HasElement("baseColor")) {
    printf("SDF for base has no baseColor configured, defaulting to RED!\n");
    base_color_ = gazsim_msgs::Color::RED;
  } else {
    std::string config_color = _sdf->GetElement("baseColor")->Get<std::string>();
    if (config_color == "RED") {
      base_color_ = gazsim_msgs::Color::RED;
    } else if (config_color == "BLACK") {
      base_color_ = gazsim_msgs::Color::BLACK;
    } else if (config_color == "SILVER") {
      base_color_ = gazsim_msgs::Color::SILVER;
    } else {
      printf("SDF for base has no baseColor configured, defaulting to RED!\n");
      base_color_ = gazsim_msgs::Color::RED;
    }
    printf("Base spawns in color %s\n", config_color.c_str());
  }
  
  delivery_pub_ = node_->Advertise<llsf_msgs::SetOrderDeliveredByColor>(TOPIC_SET_ORDER_DELIVERY_BY_COLOR);
}
void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  // Store the pointer to the model.
  model_ = _model;

  world_ = model_->GetWorld();

  namespace_.clear();

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n";

  node_handle_ = new ros::NodeHandle(namespace_);

  getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_,
                           motor_velocity_reference_pub_topic_);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboMavlinkInterface::OnUpdate, this, _1));

  mav_control_sub_ = node_handle_->subscribe(mavlink_control_sub_topic_, 10,
                                           &GazeboMavlinkInterface::MavlinkControlCallback,
                                           this);
  // Subscriber to IMU sensor_msgs::Imu Message.
  imu_sub_ = node_handle_->subscribe(imu_sub_topic_, 10, &GazeboMavlinkInterface::ImuCallback, this);
  
  motor_velocity_reference_pub_ = node_handle_->advertise<mav_msgs::Actuators>(motor_velocity_reference_pub_topic_, 10);
  hil_sensor_pub_ = node_handle_->advertise<mavros_msgs::Mavlink>(hil_sensor_mavlink_pub_topic_, 10);

  _rotor_count = 4;
  last_time_ = world_->GetSimTime();
  last_gps_time_ = world_->GetSimTime();
  double gps_update_interval_ = 200*1000000;  // nanoseconds for 5Hz

  gravity_W_ = world_->GetPhysicsEngine()->GetGravity();

  // Magnetic field data for Zurich from WMM2015 (10^5xnanoTesla (N, E, D))
  
  mag_W_.x = 0.21523;
  mag_W_.y = 0.00771;
  mag_W_.z = 0.42741;

}
Esempio n. 21
0
void RockBridge::instantiatePluginComponents(sdf::ElementPtr modelElement, ModelPtr model)
{
    sdf::ElementPtr pluginElement = modelElement->GetElement("plugin");
    while(pluginElement) {
        string pluginFilename = pluginElement->Get<string>("filename");
        gzmsg << "RockBridge: found plugin "<< pluginFilename << endl;
        // Add more model plugins testing them here
        if(pluginFilename == "libgazebo_thruster.so")
        {
            ThrusterTask* thruster_task = new ThrusterTask();
            thruster_task->setGazeboModel( model );
            setupTaskActivity(thruster_task);
        }

        pluginElement = pluginElement->GetNextElement("plugin");
    }
}
void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->model_ = _parent;
  this->world_ = this->model_->GetWorld();

  // Check for link element
  if (!_sdf->HasElement("link"))
  {
    ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
    return;
  }
  
  link_name_ = _sdf->GetElement("link")->Get<std::string>();

  // Get pointers to joints
  link_ = model_->GetLink(link_name_);
  if(link_)
    link_->SetEnabled(false);
  else
    ROS_WARN("Link %s not found!", link_name_.c_str());
}
void GazeboControllerInterface::Load(physics::ModelPtr _model,
                                     sdf::ElementPtr _sdf) {
  if (kPrintOnPluginLoad) {
    gzdbg << __FUNCTION__ << "() called." << std::endl;
  }

  // Store the pointer to the model.
  model_ = _model;

  world_ = model_->GetWorld();

  namespace_.clear();

  //==============================================//
  //========== READ IN PARAMS FROM SDF ===========//
  //==============================================//

  if (_sdf->HasElement("robotNamespace")) {
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  } else {
    gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n";
  }

  node_handle_ = gazebo::transport::NodePtr(new transport::Node());

  // Initialise with default namespace (typically /gazebo/default/)
  node_handle_->Init();

  getSdfParam<std::string>(_sdf, "commandMotorSpeedSubTopic",
                           command_motor_speed_sub_topic_,
                           command_motor_speed_sub_topic_);
  getSdfParam<std::string>(_sdf, "motorSpeedCommandPubTopic",
                           motor_velocity_reference_pub_topic_,
                           motor_velocity_reference_pub_topic_);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboControllerInterface::OnUpdate, this, _1));
}
Esempio n. 24
0
void RockBridge::instantiateSensorComponents(sdf::ElementPtr modelElement, ModelPtr model)
{
    sdf::ElementPtr linkElement = modelElement->GetElement("link");
    while( linkElement ){
        sdf::ElementPtr sensorElement = linkElement->GetElement("sensor");
        while( sensorElement ){
            string sensorName = sensorElement->Get<string>("name");
            string sensorType = sensorElement->Get<string>("type");
            // To support more sensors, test for different sensors types
            if( sensorType == "ray" )
            {
                gzmsg << "RockBridge: creating laser line component: " + sensorName << endl;
                LaserScanTask* laser_line_task = new LaserScanTask();
                string topicName = model->GetName() + "/" + linkElement->Get<string>("name") + "/" + sensorName + "/scan";
                laser_line_task->setGazeboModel( model, sensorName, topicName );
                setupTaskActivity( laser_line_task );
            }
            else if(sensorType == "camera")
            {
                gzmsg << "RockBridge: creating camera component: " + sensorName << endl;
                CameraTask* camera = new CameraTask();
                string topicName = model->GetName() + "/" + linkElement->Get<string>("name") + "/" + sensorName + "/image";
                camera->setGazeboModel(model, sensorName, topicName);
                setupTaskActivity(camera);
            }
            else if(sensorType == "imu")
            {
                gzmsg << "RockBridge: creating imu component: " + sensorName << endl;
                ImuTask *imu = new ImuTask();
                string topicName = model->GetName() + "/" + linkElement->Get<string>("name") + "/" + sensorName + "/imu";
                imu->setGazeboModel(model, sensorName, topicName);
                setupTaskActivity(imu);
            }

            sensorElement = sensorElement->GetNextElement("sensor");
        }
        linkElement = linkElement->GetNextElement("link");
    }
}
void GazeboRosElevator::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  // load parameters
  this->robotNamespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
  {
    this->robotNamespace_ = _sdf->GetElement(
        "robotNamespace")->Get<std::string>() + "/";
  }

  // Make sure the ROS node for Gazebo has already been initialized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM_NAMED("elevator", "A ROS node for Gazebo has not been initialized,"
        << "unable to load plugin. Load the Gazebo system plugin "
        << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

  std::string topic = "elevator";
  if (_sdf->HasElement("topic"))
    topic = _sdf->Get<std::string>("topic");

  ElevatorPlugin::Load(_parent, _sdf);

  this->rosnode_ = new ros::NodeHandle(this->robotNamespace_);

  ros::SubscribeOptions so =
    ros::SubscribeOptions::create<std_msgs::String>(topic, 1,
        boost::bind(&GazeboRosElevator::OnElevator, this, _1),
        ros::VoidPtr(), &this->queue_);

  this->elevatorSub_ = this->rosnode_->subscribe(so);

  // start custom queue for elevator
  this->callbackQueueThread_ =
    boost::thread(boost::bind(&GazeboRosElevator::QueueThread, this));
}
    void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        model_ = _parent;
        world_ = model_->GetWorld();

        // Check for link element
        if (!_sdf->HasElement("link")) {
            ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
            return;
        }

        link_name_ = _sdf->GetElement("link")->Get<std::string>();

        // Get pointers to joints
        link_ = model_->GetLink(link_name_);
        if (link_) {
            link_->SetEnabled(false);
            // Output some confirmation
            ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: \"" << link_name_);
        }
        else
            ROS_ERROR_STREAM("Link" << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
    }
Esempio n. 27
0
void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
  // Store the pointer to the model
  model_ = _model;
  world_ = model_->GetWorld();
  frame_id_ = "world";
  link_name_ = "base_link";
  wind_direction_ = math::Vector3(1, 0, 0);
  wind_gust_direction_ = math::Vector3(0, 1, 0);
  double wind_gust_start = 10;
  double wind_gust_duration = 0;

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n";
  node_handle_ = new ros::NodeHandle(namespace_);

  if (_sdf->HasElement("xyzOffset"))
    xyz_offset_ = _sdf->GetElement("xyzOffset")->Get<math::Vector3>();
  else
    gzerr << "[gazebo_wind_plugin] Please specify a xyzOffset.\n";

  if (_sdf->HasElement("frameId"))
    frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();

  if (_sdf->HasElement("linkName"))
    link_name_ = _sdf->GetElement("linkName")->Get<std::string>();
  else
    gzwarn << "[gazebo_wind_plugin] No linkName specified, using default " << link_name_ << ".\n";
  // Get the pointer to the link
  link_ = this->model_->GetLink(link_name_);

  // Wind params
  if (_sdf->HasElement("windDirection"))
    wind_direction_ = _sdf->GetElement("windDirection")->Get<math::Vector3>();

  if (_sdf->HasElement("windForceMean"))
    wind_force_mean_ = _sdf->GetElement("windForceMean")->Get<double>();

  if (_sdf->HasElement("windForceVariance")) {
    wind_force_variance_ = _sdf->GetElement("windForceVariance")->Get<double>();
  }

  // Wind gust params
  if (_sdf->HasElement("windGustDirection")) {
    wind_gust_direction_ = _sdf->GetElement("windGustDirection")->Get<math::Vector3>();
  }

  if (_sdf->HasElement("windGustStart"))
    wind_gust_start = _sdf->GetElement("windGustStart")->Get<double>();
  wind_gust_start_ = common::Time(wind_gust_start);

  if (_sdf->HasElement("windGustDuration")) {
    wind_gust_duration = _sdf->GetElement("windGustDuration")->Get<double>();
  }
  wind_gust_end_ = common::Time(wind_gust_start + wind_gust_duration);

  if (_sdf->HasElement("windGustForceMean")) {
    wind_gust_force_mean_ = _sdf->GetElement("windGustForceMean")->Get<double>();
  }

  if (_sdf->HasElement("windGustForceVariance")) {
    wind_gust_force_variance_ = _sdf->GetElement("windGustForceVariance")->Get<double>();
  }

  getSdfParam<std::string>(_sdf, "windPubTopic", wind_pub_topic_, "/" + namespace_ + "/wind");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboWindPlugin::OnUpdate, this, _1));

  wind_pub_ = node_handle_->advertise<geometry_msgs::WrenchStamped>(wind_pub_topic_, 10);
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
  // load plugin
  RayPlugin::Load(_parent, this->sdf);
  // Get then name of the parent sensor
  this->parent_sensor_ = _parent;
  // Get the world name.
# if GAZEBO_MAJOR_VERSION >= 7
  std::string worldName = _parent->WorldName();
# else
  std::string worldName = _parent->GetWorldName();
# endif
  this->world_ = physics::get_world(worldName);
  // save pointers
  this->sdf = _sdf;

  this->last_update_time_ = common::Time(0);

  GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
  this->parent_ray_sensor_ =
    dynamic_pointer_cast<sensors::RaySensor>(this->parent_sensor_);

  if (!this->parent_ray_sensor_)
    gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent");

  this->robot_namespace_ = "";
  if (this->sdf->HasElement("robotNamespace"))
    this->robot_namespace_ = this->sdf->Get<std::string>("robotNamespace") + "/";

  if (!this->sdf->HasElement("frameName"))
  {
    ROS_INFO("Range plugin missing <frameName>, defaults to /world");
    this->frame_name_ = "/world";
  }
  else
    this->frame_name_ = this->sdf->Get<std::string>("frameName");

  if (!this->sdf->HasElement("topicName"))
  {
    ROS_INFO("Range plugin missing <topicName>, defaults to /range");
    this->topic_name_ = "/range";
  }
  else
    this->topic_name_ = this->sdf->Get<std::string>("topicName");

  if (!this->sdf->HasElement("radiation"))
  {
      ROS_WARN("Range plugin missing <radiation>, defaults to ultrasound");
      this->radiation_ = "ultrasound";

  }
  else
      this->radiation_ = _sdf->GetElement("radiation")->Get<std::string>();

  if (!this->sdf->HasElement("fov"))
  {
      ROS_WARN("Range plugin missing <fov>, defaults to 0.05");
      this->fov_ = 0.05;
  }
  else
      this->fov_ = _sdf->GetElement("fov")->Get<double>();
  if (!this->sdf->HasElement("gaussianNoise"))
  {
    ROS_INFO("Range plugin missing <gaussianNoise>, defaults to 0.0");
    this->gaussian_noise_ = 0;
  }
  else
    this->gaussian_noise_ = this->sdf->Get<double>("gaussianNoise");

  if (!this->sdf->HasElement("updateRate"))
  {
    ROS_INFO("Range plugin missing <updateRate>, defaults to 0");
    this->update_rate_ = 0;
  }
  else
    this->update_rate_ = this->sdf->Get<double>("updateRate");

  // prepare to throttle this plugin at the same rate
  // ideally, we should invoke a plugin update when the sensor updates,
  // have to think about how to do that properly later
  if (this->update_rate_ > 0.0)
    this->update_period_ = 1.0/this->update_rate_;
  else
    this->update_period_ = 0.0;

  this->range_connect_count_ = 0;

  this->range_msg_.header.frame_id = this->frame_name_;
  if (this->radiation_==std::string("ultrasound"))
     this->range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND;
  else
      this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED;

  this->range_msg_.field_of_view = fov_;
# if GAZEBO_MAJOR_VERSION >= 7
  this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax();
  this->range_msg_.min_range = this->parent_ray_sensor_->RangeMin();
# else
  this->range_msg_.max_range = this->parent_ray_sensor_->GetRangeMax();
  this->range_msg_.min_range = this->parent_ray_sensor_->GetRangeMin();
# endif

  // Init ROS
  if (ros::isInitialized())
  {
    // ros callback queue for processing subscription
    this->deferred_load_thread_ = boost::thread(
      boost::bind(&GazeboRosRange::LoadThread, this));
  }
  else
  {
    gzerr << "Not loading plugin since ROS hasn't been "
          << "properly initialized.  Try starting gazebo with ros plugin:\n"
          << "  gazebo -s libgazebo_ros_api_plugin.so\n";
  }
}
Esempio n. 29
0
void MimicJointPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  ros::NodeHandle model_nh;
  model_ = _parent;
  world_ = model_->GetWorld();

  // Error message if the model couldn't be found
  if (!model_)
  {
    ROS_ERROR("Parent model is NULL! GazeboNaoqiControlPlugin could not be loaded.");
    return;
  }

  // Check that ROS has been initialized
  if(!ros::isInitialized())
  {
    ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin.");
    return;
  }

  // Check for robot namespace
  robot_namespace_ = "/";
  if(_sdf->HasElement("robotNamespace"))
  {
    robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  }

  // Check for joint element
  if(!_sdf->HasElement("joint"))
  {
    ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded.");
    return;
  }
  
  joint_name_ = _sdf->GetElement("joint")->Get<std::string>();

  // Check for mimicJoint element
  if(!_sdf->HasElement("mimicJoint"))
  {
    ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded.");
    return;
  }
  
  mimic_joint_name_ = _sdf->GetElement("mimicJoint")->Get<std::string>();

  has_pid_ = false;
  // Check if PID controller wanted
  if(_sdf->HasElement("hasPID"))
  {
    has_pid_ = true;

    const ros::NodeHandle nh(model_nh, std::string(robot_namespace_+"/gazebo_ros_control/pid_gains/")+mimic_joint_name_);
    double p, i,d ;
    // TO-DO: include i_clamp e.t.c.
    nh.param("p", p, 0.0);
    nh.param("i", i, 0.0);
    nh.param("d", d, 0.0);

    pid_ = control_toolbox::Pid(p,i,d);
  }

  // Check for multiplier element
  multiplier_ = 1.0;
  if(_sdf->HasElement("multiplier"))
    multiplier_ = _sdf->GetElement("multiplier")->Get<double>();

  // Check for offset element
  offset_ = 0.0;
  if (_sdf->HasElement("offset"))
    offset_ = _sdf->GetElement("offset")->Get<double>();

  // Check for sensitiveness element
  sensitiveness_ = 0.0;
  if (_sdf->HasElement("sensitiveness"))
    sensitiveness_ = _sdf->GetElement("sensitiveness")->Get<double>();

  // Check for max effort
  max_effort_ = 1.0;
  if (_sdf->HasElement("maxEffort"))
  {
    max_effort_ = _sdf->GetElement("maxEffort")->Get<double>();
  }

  // Get pointers to joints
  joint_ = model_->GetJoint(joint_name_);
  if(!joint_)
  {
    ROS_ERROR("No joint named %s. MimicJointPlugin could not be loaded.", joint_name_.c_str());
    return;
  }
  mimic_joint_ = model_->GetJoint(mimic_joint_name_);
  if(!mimic_joint_)
  {
    ROS_ERROR("No (mimic) joint named %s. MimicJointPlugin could not be loaded.", mimic_joint_name_.c_str());
    return;
  }
  
  // Set max effort
  if(!has_pid_)
    mimic_joint_->SetMaxForce(0,max_effort_);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&MimicJointPlugin::UpdateChild, this));
}
void OpticalFlowPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
  if (!_sensor)

    gzerr << "Invalid sensor pointer.\n";

  this->parentSensor =
#if GAZEBO_MAJOR_VERSION >= 7
    std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#else
    boost::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#endif

  if (!this->parentSensor)
  {
    gzerr << "OpticalFlowPlugin requires a CameraSensor.\n";
#if GAZEBO_MAJOR_VERSION >= 7
    if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#else
    if (boost::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#endif
      gzmsg << "It is a depth camera sensor\n";
  }

#if GAZEBO_MAJOR_VERSION >= 7
  this->camera = this->parentSensor->Camera();
#else
  this->camera = this->parentSensor->GetCamera();
#endif

  if (!this->parentSensor)
  {
    gzerr << "OpticalFlowPlugin not attached to a camera sensor\n";
    return;
  }

#if GAZEBO_MAJOR_VERSION >= 7
  this->width = this->camera->ImageWidth();
  this->height = this->camera->ImageHeight();
  this->depth = this->camera->ImageDepth();
  this->format = this->camera->ImageFormat();
#else
  this->width = this->camera->GetImageWidth();
  this->height = this->camera->GetImageHeight();
  this->depth = this->camera->GetImageDepth();
  this->format = this->camera->GetImageFormat();
#endif


  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_optical_flow_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);
  // TODO(tfoote) Find a way to namespace this within the model to allow multiple models
  opticalFlow_pub_ = node_handle_->Advertise<opticalFlow_msgs::msgs::opticalFlow>(topicName, 10);



  this->newFrameConnection = this->camera->ConnectNewImageFrame(
      boost::bind(&OpticalFlowPlugin::OnNewFrame, this, _1, this->width, this->height, this->depth, this->format));

  this->parentSensor->SetActive(true);
}