// Update the controller
void GazeboRosDiffDrive::UpdateChild()
{
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
// Update robot in case new velocities have been requested
getWheelVelocities();
double current_speed[2];
current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
if ( wheel_accel == 0 ||
( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) ||
( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) {
//if max_accel == 0, or target speed is reached
joints_[LEFT]->SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
} else {
if ( wheel_speed_[LEFT]>=current_speed[LEFT] )
wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update );
if ( wheel_speed_[RIGHT]>current_speed[RIGHT] )
wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update );
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
joints_[LEFT]->SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
}
last_update_time_+= common::Time ( update_period_ );
}
}
// Update the controller
void GazeboRosTricycleDrive::UpdateChild()
{
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
double target_wheel_roation_speed = cmd_.speed / ( diameter_actuated_wheel_ / 2.0 );
double target_steering_angle = cmd_.angle;
motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update );
//ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle);
last_actuator_update_ += common::Time ( update_period_ );
}
}
// Update the controller
void GazeboRosDiffDrive::UpdateChild()
{
/* force reset SetMaxForce since Joint::Reset reset MaxForce to zero at
https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331
(this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 )
and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset
(this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e)
*/
for ( int i = 0; i < 2; i++ ) {
if ( fabs(wheel_torque -joints_[i]->GetParam ("fmax", 0 )) > 1e-6 ) {
joints_[i]->SetParam("fmax", 0, wheel_torque );
}
}
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
// Update robot in case new velocities have been requested
getWheelVelocities();
double current_speed[2];
current_speed[LEFT ] = joints_[LEFT ]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
if ( wheel_accel == 0 ||
( ( fabs ( wheel_speed_[LEFT ] - current_speed[LEFT ] ) < 0.01 ) &&
( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) ) {
//if max_accel == 0, or target speed is reached
wheel_speed_instr_[LEFT ] = wheel_speed_[LEFT ];
wheel_speed_instr_[RIGHT] = wheel_speed_[RIGHT];
} else {
if ( wheel_speed_[LEFT ] >= current_speed[LEFT ] )
wheel_speed_instr_[LEFT ] += fmin ( wheel_speed_[LEFT ]-current_speed[LEFT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[LEFT ] += fmax ( wheel_speed_[LEFT ]-current_speed[LEFT], -wheel_accel * seconds_since_last_update );
if ( wheel_speed_[RIGHT] > current_speed[RIGHT] )
wheel_speed_instr_[RIGHT] += fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[RIGHT] += fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update );
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
}
wheel_applied_vel[LEFT ] = wheel_speed_instr_[LEFT ] / ( wheel_diameter_ / 2.0 );
wheel_applied_vel[RIGHT] = wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 );
last_update_time_ = current_time;
joints_[LEFT ]->SetParam("vel", 0, wheel_applied_vel[LEFT ] );
joints_[RIGHT]->SetParam("vel", 0, wheel_applied_vel[RIGHT] );
}
}
