NishidalabYpspurDriver::NishidalabYpspurDriver() : 
  linear_vel_max(1.1),
  angular_vel_max(M_PI),
  linear_acc_max(1.0),
  angular_acc_max(M_PI)
{
  // init yp-spur---------------------------------------------------------------------
  if( Spur_init() < 0)
    ROS_ERROR("can't open ypspur");

  // using parameter server
  n.param("nishidalab_ypspur/linear_vel_max", linear_vel_max, linear_vel_max);
  n.param("nishidalab_ypspur/angular_vel_max", angular_vel_max, angular_vel_max);
  n.param("nishidalab_ypspur/linear_acc_max", linear_acc_max, linear_acc_max);
  n.param("nishidalab_ypspur/angular_acc_max", angular_acc_max, angular_acc_max);
  // init velocity & accelaration limits (Unit is m/s & rad/s)
  Spur_set_vel(linear_vel_max);
  Spur_set_accel(linear_acc_max);
  Spur_set_angvel(angular_vel_max);
  Spur_set_angaccel(angular_acc_max);
  // end init yp-spur------------------------------------------------------------------

  cmd_vel_sub = n.subscribe<geometry_msgs::Twist>("cmd_vel", 10, &NishidalabYpspurDriver::Callback, this);

}
Exemplo n.º 2
0
int main(void)
{
  if (!Spur_init()) {
    fprintf(stderr, "ERROR : cannot open spur\n");
    return -1;
  }

  // Setting up signal to stops when entering Ctrl-C.
  setSigInt();

  // Setting up velocity.
  Spur_set_vel(0.3);

  // Setting up acceleration.
  Spur_set_accel(1.0);

  // Setting up angular velocity.
  Spur_set_angvel(1.5);

  // Setting up angular acceleration.
  Spur_set_angaccel(2.0);

  // Setting up starting point.
  Spur_set_pos_GL(0, 0, 0);
  

  // <Write here>

  // Stop!
  Spur_stop();

  return 0;
}