New to ROS, but I am trying to get the tricycle model plugin to accurately respond to rqt commands and update its position in rviz.
I was able to get good response to rqt commands in Gazebo by:
1. Amending the GazeboTricycleDrive::motorController function to account for forward and reverse motion (so, when moving backwards, it flips acceleration and deceleration), and
2. Changing from '+/- steering_speed_' based angle corrections to something like below:
if ( steering_speed_ > 0 ) {
double diff_angle = current_angle - target_angle;
if ( fabs ( diff_angle ) < steering_angle_tolerance_ ) {
applied_angle = target_angle;
// applied_steering_speed = 0;
} else if ( diff_angle < target_angle ) {
applied_angle = current_angle + steering_speed_ * dt;
// applied_steering_speed = steering_speed_;
} else {
applied_angle = current_angle - steering_speed_ * dt;
// applied_steering_speed = -steering_speed_;
}
}
Despite this fix helping to get the tricycle responding well with rqt commands, it produces strange artefacts in rviz --- when rotating, the walls all appear to rotate with the tricycle. It would appear that the tricycle doesn't know that it's rotating, and continues to update its map as though it were always facing forward (see [screenshots](https://app.box.com/s/ea2v9o15k5pm013v22hj6gyqsr0en368)).
The GazeboTricycleDrive::UpdateOdometryEncoder() function appears to be using the exact same physics as the differential drive plugin. I have a suspicion that this is where the error is creeping in: the rear wheels are rotating at the same speeds and it will therefore never register any rotation from the encoders.
I tried to gut the existing UpdateOdometryEncoder() code and replace the pose_encoder_.x, pose_encoder_.y, pose_encoder_.theta updates with dx, dy, and dtheta values deriving from these sources:
[Source 1](http://iiith.vlab.co.in/?sub=21&brch=72&sim=511&cnt=1)
[Source 2](https://books.google.ca/books?id=gmYALDVqlLUC&pg=PA49&lpg=PA49&dq=angular+velocity+of+tricycle+steering+wheel&source=bl&ots=53HIjp4eis&sig=Nu8nyx63-1kaZgLDtlXAe-Xdwzo&hl=en&sa=X&ved=0ahUKEwjwureH-s7QAhVI9IMKHbTpDK4Q6AEISTAJ#v=onepage&q=angular%20velocity%20of%20tricycle%20steering%20wheel&f=false)
My code looks something like:
// Calculate odom_ based on steering wheel angle/velocity
double angle_steer = joint_steering_->GetAngle ( 0 ).Radian();
double current_speed = joint_wheel_actuated_->GetVelocity ( 0 );
// From http://iiith.vlab.co.in/?sub=21&brch=72&sim=511&cnt=1
/*
Steering angle = alpha; separation_steering_wheel_ = B; separation_encoder_wheel_ = 2*d; steering wheel speed = V_F
Rate of rotation about ICC : omega = V_F * sin ( alpha ) / B
Turning radius : R = B / tan ( alpha )
Angular velocity : V = omega * R = V_F * cos ( alpha )
Left wheel speed : V_L = omega * ( R + d ) = V + d * V_F * sin ( alpha ) / B
Right wheel speed : V_R = omega * ( R - d ) = V - d * V_F * sin ( alpha ) / B
*/
double omega = current_speed * sin ( angle_steer ) / separation_steering_wheel_;
double ang_vel = current_speed * cos ( angle_steer );
double vl = ang_vel + d * current_speed * sin ( angle_steer ) / separation_steering_wheel_;
double vr = ang_vel + d * current_speed * sin ( angle_steer ) / separation_steering_wheel_;
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_odom_update_ ).Double();
last_odom_update_ = current_time;
double dx = current_speed * cos ( angle_steer ) * cos ( pose_encoder_.theta ) * seconds_since_last_update;
double dy = current_speed * cos ( angle_steer ) * sin ( pose_encoder_.theta ) * seconds_since_last_update;
double dtheta = current_speed / separation_steering_wheel_ * sin ( angle_steer ) * seconds_since_last_update;
But this doesn't fix the problem of the moving map, and it also creates some wonky looking movement in rviz.
IF my logic is fine, should I be grabbing my angles from elsewhere? I get the sense using pose_encoder_.theta isn't strictly kosher.
Any help is appreciated. Please let me know if there's any other information you need.
EDIT: To clarify, the moving walls were present even with the original plugin code.
↧