grizzly_plugin.cpp

Go to the documentation of this file.

#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Twist.h>
#include <grizzly_plugin/grizzly_plugin.h>
#include <ros/time.h>

using namespace gazebo;

enum {BL= 0, BR=1, FL=2, FR=3, FA=4};

GrizzlyPlugin::GrizzlyPlugin()
{
}

GrizzlyPlugin::~GrizzlyPlugin()
{
  delete rosnode_;
  delete spinner_thread_;
}

void GrizzlyPlugin::FiniChild()
{
  rosnode_->shutdown();
  spinner_thread_->join();
}
    
void GrizzlyPlugin::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")->Get<std::string>() + "/";


  bl_joint_name_ = "backLeftJoint";
  if (_sdf->HasElement("backLeftJoint"))
    bl_joint_name_ = _sdf->GetElement("backLeftJoint")->Get<std::string>();

  br_joint_name_ = "backRightJoint";
  if (_sdf->HasElement("backRightJoint"))
    br_joint_name_ = _sdf->GetElement("backRightJoint")->Get<std::string>();

  fl_joint_name_ = "frontLeftJoint";
  if (_sdf->HasElement("frontLeftJoint"))
    fl_joint_name_ = _sdf->GetElement("frontLeftJoint")->Get<std::string>();

  fr_joint_name_ = "frontRightJoint";
  if (_sdf->HasElement("frontRightJoint"))
    fr_joint_name_ = _sdf->GetElement("frontRightJoint")->Get<std::string>();

  fa_joint_name_ = "frontAxleJoint";
  if (_sdf->HasElement("frontAxleJoint"))
    fa_joint_name_ = _sdf->GetElement("frontAxleJoint")->Get<std::string>();

  torque_ = 15.0;
  if (_sdf->HasElement("torque"))
    torque_ = _sdf->GetElement("torque")->Get<double>();

  base_geom_name_ = "base_link";
  if (_sdf->HasElement("baseGeom"))
    base_geom_name_ = _sdf->GetElement("baseGeom")->Get<std::string>();
  base_geom_ = model_->GetChildCollision(base_geom_name_);


  //base_geom_->SetContactsEnabled(true);

  // Get the name of the parent model
  std::string modelName = _sdf->GetParent()->Get<std::string>("name");
  gzdbg << "plugin model name: " << modelName << "\n";

  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);

  js_.name.push_back( fa_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;

  wheel_ang_vel_.rear_left = 0.0;
  wheel_ang_vel_.rear_right= 0.0;
  wheel_ang_vel_.front_left = 0.0;
  wheel_ang_vel_.front_left = 0.0;

  set_joints_[0] = false;
  set_joints_[1] = false;
  set_joints_[2] = false;
  set_joints_[3] = false;
  set_joints_[4] = false;

  //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_);
  joints_[FA] = model_->GetJoint(fa_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;
  if (joints_[FA]) set_joints_[FA] = true;

  //initialize time and odometry position
  prev_update_time_ = last_cmd_vel_time_ = this->world_->GetSimTime();

  // Initialize the ROS node and subscribe to cmd_vel
  int argc = 0;
  char** argv = NULL;
  ros::init(argc, argv, "gazebo_grizzly", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  rosnode_ = new ros::NodeHandle( node_namespace_ );

  drive_sub_ = rosnode_->subscribe("cmd_drive", 1, &GrizzlyPlugin::OnDrive, this );
  encoder_pub_  = rosnode_->advertise<grizzly_msgs::Drive>("motors/encoders", 1);
  joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("joint_states", 1);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->spinner_thread_ = new boost::thread( boost::bind( &GrizzlyPlugin::spin, this) );
  this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&GrizzlyPlugin::UpdateChild, this));
}


void GrizzlyPlugin::UpdateChild()
{
  common::Time time_now = this->world_->GetSimTime();
  common::Time step_time = time_now - prev_update_time_;
  prev_update_time_ = time_now;
  grizzly_msgs::Drive encoder_msg;

  // set joint velocity based on teleop input and publish joint states
  js_.header.stamp.sec = time_now.sec;
  js_.header.stamp.nsec = time_now.nsec;
  if (set_joints_[BL])
  {
    joints_[BL]->SetVelocity( 0, wheel_ang_vel_.rear_left);
    joints_[BL]->SetMaxForce( 0, torque_ );
    js_.position[0] = joints_[BL]->GetAngle(0).Radian();
    js_.velocity[0] = encoder_msg.rear_left = joints_[BL]->GetVelocity(0);
  }
  if (set_joints_[BR])
  {
    joints_[BR]->SetVelocity( 0, wheel_ang_vel_.rear_right);
    joints_[BR]->SetMaxForce( 0, torque_ );
    js_.position[1] = joints_[BR]->GetAngle(0).Radian();
    js_.velocity[1] = encoder_msg.rear_right = joints_[BR]->GetVelocity(0);
  }
  if (set_joints_[FL])
  {
    joints_[FL]->SetVelocity( 0, wheel_ang_vel_.front_left);
    joints_[FL]->SetMaxForce( 0, torque_ );
    js_.position[3] = joints_[FR]->GetAngle(0).Radian();
    js_.velocity[3] = encoder_msg.front_right = joints_[FR]->GetVelocity(0);
  }
  if (set_joints_[FR])
  {
    joints_[FR]->SetVelocity( 0, wheel_ang_vel_.front_right);
    joints_[FR]->SetMaxForce( 0, torque_ );
    js_.position[2] = joints_[FL]->GetAngle(0).Radian();
    js_.velocity[2] = encoder_msg.front_left = joints_[FL]->GetVelocity(0);
  }

  if (set_joints_[FA])
  {
    js_.position[4] = joints_[FA]->GetAngle(0).Radian();
    js_.velocity[4] = joints_[FA]->GetVelocity(0);
  }

  // publish joint states
  joint_state_pub_.publish( js_ );

  // publish encoder message
  encoder_msg.header.stamp.sec = time_now.sec;
  encoder_msg.header.stamp.nsec = time_now.nsec;
  encoder_pub_.publish( encoder_msg );

  // Timeout if we haven't received a cmd in <0.1 s
  common::Time time_since_last_cmd = time_now - last_cmd_vel_time_;
  if (time_since_last_cmd.Double() > 0.1)
  {
    wheel_ang_vel_.rear_left = 0;
    wheel_ang_vel_.rear_right = 0;
    wheel_ang_vel_.front_left = 0;
    wheel_ang_vel_.front_right = 0;
  }
}


void GrizzlyPlugin::OnDrive( const grizzly_msgs::DriveConstPtr &msg)
{
  last_cmd_vel_time_ = this->world_->GetSimTime();  
  wheel_ang_vel_ = *msg;
}

void GrizzlyPlugin::spin()
{
  while(ros::ok()) ros::spinOnce();
}

GZ_REGISTER_MODEL_PLUGIN(GrizzlyPlugin);