Ejemplo n.º 1
0
void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->my_world_ = _parent->GetWorld();

  this->my_parent_ = _parent;
  if (!this->my_parent_)
  {
    ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
    return;
  }


  this->node_namespace_ = "";
  if (_sdf->HasElement("node_namespace"))
    this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";


  left_wheel_joint_name_ = "left_wheel_joint";
  if (_sdf->HasElement("left_wheel_joint"))
    left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();

  right_wheel_joint_name_ = "right_wheel_joint";
  if (_sdf->HasElement("right_wheel_joint"))
    right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();

  front_castor_joint_name_ = "front_castor_joint";
  if (_sdf->HasElement("front_castor_joint"))
    front_castor_joint_name_ = _sdf->GetElement("front_castor_joint")->GetValueString();

  rear_castor_joint_name_ = "rear_castor_joint";
  if (_sdf->HasElement("rear_castor_joint"))
    rear_castor_joint_name_ = _sdf->GetElement("rear_castor_joint")->GetValueString();

  wheel_sep_ = 0.34;
  if (_sdf->HasElement("wheel_separation"))
    wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();

  wheel_sep_ = 0.34;
  if (_sdf->HasElement("wheel_separation"))
    wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();

  wheel_diam_ = 0.15;
  if (_sdf->HasElement("wheel_diameter"))
    wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();

  torque_ = 10.0;
  if (_sdf->HasElement("torque"))
    torque_ = _sdf->GetElement("torque")->GetValueDouble();


  //base_geom_name_ = "base_geom";
  base_geom_name_ = "base_footprint_geom_base_link";
  if (_sdf->HasElement("base_geom"))
    base_geom_name_ = _sdf->GetElement("base_geom")->GetValueString();
  base_geom_ = my_parent_->GetChildCollision(base_geom_name_);
  if (!base_geom_)
  {
    // This is a hack for ROS Diamond back. E-turtle and future releases
    // will not need this, because it will contain the fixed-joint reduction
    // in urdf2gazebo
    base_geom_ = my_parent_->GetChildCollision("base_footprint_geom");
    if (!base_geom_)
    {
      ROS_ERROR("Unable to find geom[%s]",base_geom_name_.c_str());
      return;
    }
  }

  base_geom_->SetContactsEnabled(true);
  contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));

  // Get then name of the parent model
  std::string modelName = _sdf->GetParent()->GetValueString("name");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosCreate::UpdateChild, this));
  gzdbg << "plugin model name: " << modelName << "\n";

  /*wall_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
    sensors::SensorManager::Instance()->GetSensor("wall_sensor"));
  if (!wall_sensor_)
  {
    ROS_ERROR("Unable to find sensor[wall_sensor]");
    return;
  }

  left_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
    sensors::SensorManager::Instance()->GetSensor("left_cliff_sensor"));
  right_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
    sensors::SensorManager::Instance()->GetSensor("right_cliff_sensor"));
  leftfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
    sensors::SensorManager::Instance()->GetSensor("leftfront_cliff_sensor"));
  rightfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
    sensors::SensorManager::Instance()->GetSensor("rightfront_cliff_sensor"));*/

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

  rosnode_ = new ros::NodeHandle( node_namespace_ );

  cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosCreate::OnCmdVel, this );

  sensor_state_pub_ = rosnode_->advertise<turtlebot_node::TurtlebotSensorState>("sensor_state", 1);
  odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);

  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states_roomba", 1);

  js_.name.push_back( left_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( right_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( front_castor_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( rear_castor_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  prev_update_time_ = 0;
  last_cmd_vel_time_ = 0;


  sensor_state_.bumps_wheeldrops = 0x0;

  //TODO: fix this

  joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
  joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
  joints_[FRONT] = my_parent_->GetJoint(front_castor_joint_name_);
  joints_[REAR] = my_parent_->GetJoint(rear_castor_joint_name_);

  if (joints_[LEFT]) set_joints_[LEFT] = true;
  if (joints_[RIGHT]) set_joints_[RIGHT] = true;
  if (joints_[FRONT]) set_joints_[FRONT] = true;
  if (joints_[REAR]) set_joints_[REAR] = true;

  //initialize time and odometry position
  prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
  odom_pose_[0] = 0.0;
  odom_pose_[1] = 0.0;
  odom_pose_[2] = 0.0;
}
void GazeboRosEklavya::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->my_world_ = _parent->GetWorld();

  this->my_parent_ = _parent;
  if (!this->my_parent_)
  {
    ROS_FATAL("Gazebo_ROS_Eklavya controller requires a Model as its parent");
    return;
  }


  this->node_namespace_ = "";
  if (_sdf->HasElement("node_namespace"))
    this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";


  left_wheel_joint_name_ = "left_wheel_joint";
  if (_sdf->HasElement("left_wheel_joint"))
    left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();

  right_wheel_joint_name_ = "right_wheel_joint";
  if (_sdf->HasElement("right_wheel_joint"))
    right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();

  front_castor_wheel_joint_name_ = "front_castor_wheel_joint";
  if (_sdf->HasElement("front_castor_wheel_joint"))
    front_castor_wheel_joint_name_ = _sdf->GetElement("front_castor_wheel_joint")->GetValueString();

  front_castor_swivel_joint_name_ = "front_castor_swivel_joint";
  if (_sdf->HasElement("front_castor_swivel_joint"))
    front_castor_swivel_joint_name_ = _sdf->GetElement("front_castor_swivel_joint")->GetValueString();

  wheel_sep_ = 0.34;
  if (_sdf->HasElement("wheel_separation"))
    wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();

  wheel_sep_ = 0.34;
  if (_sdf->HasElement("wheel_separation"))
    wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();

  wheel_diam_ = 0.15;
  if (_sdf->HasElement("wheel_diameter"))
    wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();

  torque_ = 10.0;
  if (_sdf->HasElement("torque"))
    torque_ = _sdf->GetElement("torque")->GetValueDouble();


  
  // Get then name of the parent model
  std::string modelName = _sdf->GetParent()->GetValueString("name");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosEklavya::UpdateChild, this));
  gzdbg << "plugin model name: " << modelName << "\n";





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

  rosnode_ = new ros::NodeHandle( node_namespace_ );

  cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosEklavya::OnCmdVel, this );
  odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);

  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);

  js_.name.push_back( left_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( right_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( front_castor_wheel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( front_castor_swivel_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  prev_update_time_ = 0;
  last_cmd_vel_time_ = 0;



  //TODO: fix this

  joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
  joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
  joints_[FRONT_CASTOR_WHEEL] = my_parent_->GetJoint(front_castor_wheel_joint_name_);
  joints_[FRONT_CASTOR_SWIVEL] = my_parent_->GetJoint(front_castor_swivel_joint_name_);

  if (joints_[LEFT]) set_joints_[LEFT] = true;
  if (joints_[RIGHT]) set_joints_[RIGHT] = true;
  if (joints_[FRONT_CASTOR_WHEEL]) set_joints_[FRONT_CASTOR_WHEEL] = true;
  if (joints_[FRONT_CASTOR_SWIVEL]) set_joints_[FRONT_CASTOR_SWIVEL] = true;

  //initialize time and odometry position
  prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
  odom_pose_[0] = 0.0;
  odom_pose_[1] = 0.0;
  odom_pose_[2] = 0.0;
}
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
  model_ = parent;
  if (!model_)
  {
    ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
    return;
  }
  // Get then name of the parent model and use it as node name
  std::string model_name = sdf->GetParent()->GetValueString("name");
  gzdbg << "Plugin model name: " << model_name << "\n";
  nh_ = ros::NodeHandle("");
  // creating a private name pace until Gazebo implements topic remappings
  nh_priv_ = ros::NodeHandle("/" + model_name);
  node_name_ = model_name;

  world_ = parent->GetWorld();

// TODO: use when implementing subs
//  ros_spinner_thread_ = new boost::thread(boost::bind(&GazeboRosKobuki::spin, this));
//
//  this->node_namespace_ = "";
//  if (_sdf->HasElement("node_namespace"))
//    this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";

  /*
   * Prepare receiving motor power commands
   */
  motor_power_sub_ = nh_priv_.subscribe("commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
  motors_enabled_ = true;

  /*
   * Prepare joint state publishing
   */
  if (sdf->HasElement("left_wheel_joint_name"))
  {
    left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
                     << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("right_wheel_joint_name"))
  {
    right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
                     << " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
    return;
  }
  joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
  joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
  if (!joints_[LEFT] || !joints_[RIGHT])
  {
    ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
    return;
  }
  joint_state_.header.frame_id = "Joint States";
  joint_state_.name.push_back(left_wheel_joint_name_);
  joint_state_.position.push_back(0);
  joint_state_.velocity.push_back(0);
  joint_state_.effort.push_back(0);
  joint_state_.name.push_back(right_wheel_joint_name_);
  joint_state_.position.push_back(0);
  joint_state_.velocity.push_back(0);
  joint_state_.effort.push_back(0);
  joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);

  /*
   * Prepare publishing odometry data
   */
  if (sdf->HasElement("wheel_separation"))
  {
    wheel_sep_ = sdf->GetElement("wheel_separation")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("wheel_diameter"))
  {
    wheel_diam_ = sdf->GetElement("wheel_diameter")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("torque"))
  {
    torque_ = sdf->GetElement("torque")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);

  /*
   * Prepare receiving velocity commands
   */
  if (sdf->HasElement("velocity_command_timeout"))
  {
    cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  last_cmd_vel_time_ = world_->GetSimTime();
  cmd_vel_sub_ = nh_priv_.subscribe("commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);

  /*
   * Prepare cliff sensors
   */
  std::string cliff_sensor_left_name, cliff_sensor_front_name, cliff_sensor_right_name;
  if (sdf->HasElement("cliff_sensor_left_name"))
  {
    cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("cliff_sensor_front_name"))
  {
    cliff_sensor_front_name = sdf->GetElement("cliff_sensor_front_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("cliff_sensor_right_name"))
  {
    cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                       sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
  cliff_sensor_front_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                        sensors::SensorManager::Instance()->GetSensor(cliff_sensor_front_name));
  cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
                        sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
  if (!cliff_sensor_left_)
  {
    ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
    return;
  }
  if (!cliff_sensor_front_)
  {
    ROS_ERROR_STREAM("Couldn't find the frontal cliff sensor in the model! [" << node_name_ <<"]");
    return;
  }
  if (!cliff_sensor_right_)
  {
    ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
    return;
  }
  if (sdf->HasElement("cliff_detection_threshold"))
  {
    cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->GetValueDouble();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  cliff_sensor_left_->SetActive(true);
  cliff_sensor_front_->SetActive(true);
  cliff_sensor_right_->SetActive(true);
  cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>("events/cliff", 1);

  /*
   * Prepare bumper
   */
  std::string bumper_name;
  if (sdf->HasElement("bumper_name"))
  {
    bumper_name = sdf->GetElement("bumper_name")->GetValueString();
  }
  else
  {
    ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
                     << " Did you specify it?" << " [" << node_name_ <<"]");
    return;
  }
  bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
            sensors::SensorManager::Instance()->GetSensor(bumper_name));
  bumper_->SetActive(true);
  bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>("events/bumper", 1);

  prev_update_time_ = world_->GetSimTime();
  ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
  update_connection_ = event::Events::ConnectWorldUpdateStart(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
void GazeboRosControllerManager::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
  // Get then name of the parent model
  std::string modelName = _sdf->GetParent()->GetValueString("name");

  // Get the world name.
  this->world = _parent->GetWorld();

  // Get a pointer to the model
  this->parent_model_ = _parent;

  // Error message if the model couldn't be found
  if (!this->parent_model_)
    gzerr << "Unable to get parent model\n";

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&GazeboRosControllerManager::UpdateChild, this));
  gzdbg << "plugin model name: " << modelName << "\n";

  physics_ = false;
  this->world->EnablePhysicsEngine(physics_); //TODO should be false


  if (getenv("CHECK_SPEEDUP"))
  {
    wall_start_ = this->world->GetRealTime().Double();
    sim_start_  = this->world->GetSimTime().Double();
  }

  // check update rate against world physics update rate
  // should be equal or higher to guarantee the wrench applied is not "diluted"
  //if (this->updatePeriod > 0 &&
  //    (this->world->GetPhysicsEngine()->GetUpdateRate() > 1.0/this->updatePeriod))
  //  ROS_ERROR("gazebo_ros_force controller update rate is less than physics update rate, wrench applied will be diluted (applied intermittently)");





  // get parameter name
  this->robotNamespace = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString();

  this->robotParam = "robot_description";
  if (_sdf->HasElement("robotParam"))
    this->robotParam = _sdf->GetElement("robotParam")->GetValueString();

  this->robotParam = this->robotNamespace+"/" + this->robotParam;

  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }
  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
  ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",this->robotNamespace.c_str());

  // pr2_etherCAT calls ros::spin(), we'll thread out one spinner here to mimic that
  this->ros_spinner_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerROSThread,this ) );


  // load a controller manager
  this->cm_ = new pr2_controller_manager::ControllerManager(&hw_,*this->rosnode_);
  this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
  if (this->hw_.current_time_ < ros::Time(0.001)) this->hw_.current_time_ == ros::Time(0.001); // hardcoded to minimum of 1ms on start up

  this->rosnode_->param("gazebo/start_robot_calibrated",this->fake_calibration_,true);

  // read pr2 urdf
  // setup actuators, then setup mechanism control node
  ReadPr2Xml();

  // Initializes the fake state (for running the transmissions backwards).
  this->fake_state_ = new pr2_mechanism_model::RobotState(&this->cm_->model_);

  // The gazebo joints and mechanism joints should match up.
  if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
  {
    for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
    {
      std::string joint_name = this->cm_->state_->joint_states_[i].joint_->name;

      // fill in gazebo joints pointer
      gazebo::physics::JointPtr joint = this->parent_model_->GetJoint(joint_name);
      if (joint)
      {
        this->joints_.push_back(joint);
      }
      else
      {
        //ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
        //this->joints_.push_back(NULL);  // FIXME: cannot be null, must be an empty boost shared pointer
        this->joints_.push_back(gazebo::physics::JointPtr());  // FIXME: cannot be null, must be an empty boost shared pointer
        ROS_ERROR("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
      }

    }
  }

#ifdef USE_CBQ
  // start custom queue for controller manager
  this->controller_manager_callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerQueueThread,this ) );
#endif
}
Ejemplo n.º 5
0
void HuskyPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->model_ = _parent;
  this->world_ = this->model_->GetWorld();

  this->node_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->node_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";


  bl_joint_name_ = "backLeftJoint";
  if (_sdf->HasElement("backLeftJoint"))
    bl_joint_name_ = _sdf->GetElement("backLeftJoint")->GetValueString();

  br_joint_name_ = "backRightJoint";
  if (_sdf->HasElement("backRightJoint"))
    br_joint_name_ = _sdf->GetElement("backRightJoint")->GetValueString();

  fl_joint_name_ = "frontLeftJoint";
  if (_sdf->HasElement("frontLeftJoint"))
    fl_joint_name_ = _sdf->GetElement("frontLeftJoint")->GetValueString();

  fr_joint_name_ = "frontRightJoint";
  if (_sdf->HasElement("frontRightJoint"))
    fr_joint_name_ = _sdf->GetElement("frontRightJoint")->GetValueString();

  wheel_sep_ = 0.44;
  if (_sdf->HasElement("wheelSeparation"))
    wheel_sep_ = _sdf->GetElement("wheelSeparation")->GetValueDouble();

  wheel_diam_ = 0.25;
  if (_sdf->HasElement("wheelDiameter"))
    wheel_diam_ = _sdf->GetElement("wheelDiameter")->GetValueDouble();

  torque_ = 24.0;   // realer Wert 6
  if (_sdf->HasElement("torque"))
    torque_ = _sdf->GetElement("torque")->GetValueDouble();


  base_geom_name_ = "base_link";
  if (_sdf->HasElement("baseGeom"))
    base_geom_name_ = _sdf->GetElement("baseGeom")->GetValueString();
  base_geom_ = model_->GetChildCollision(base_geom_name_);

  //base_geom_->SetContactsEnabled(true);

  // Get the name of the parent model
  std::string modelName = _sdf->GetParent()->GetValueString("name");

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->updateConnection = event::Events::ConnectWorldUpdateStart(
      boost::bind(&HuskyPlugin::UpdateChild, this));
  gzdbg << "plugin model name: " << modelName << "\n";

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

  rosnode_ = new ros::NodeHandle( node_namespace_ );

  cmd_vel_sub_ = rosnode_->subscribe("cmd_vel", 1, &HuskyPlugin::OnCmdVel, this );

  odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);

  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("joint_states", 1);

  js_.name.push_back( bl_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( br_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( fl_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  js_.name.push_back( fr_joint_name_ );
  js_.position.push_back(0);
  js_.velocity.push_back(0);
  js_.effort.push_back(0);

  prev_update_time_ = 0;
  last_cmd_vel_time_ = 0;


  //TODO: fix this

  joints_[BL] = model_->GetJoint(bl_joint_name_);
  joints_[BR] = model_->GetJoint(br_joint_name_);
  joints_[FL] = model_->GetJoint(fl_joint_name_);
  joints_[FR] = model_->GetJoint(fr_joint_name_);

  if (joints_[BL]) set_joints_[BL] = true;
  if (joints_[BR]) set_joints_[BR] = true;
  if (joints_[FL]) set_joints_[FL] = true;
  if (joints_[FR]) set_joints_[FR] = true;

  //initialize time and odometry position
  prev_update_time_ = last_cmd_vel_time_ = this->world_->GetSimTime();
  odom_pose_[0] = 0.0;
  odom_pose_[1] = 0.0;
  odom_pose_[2] = 0.0;
}