void GazeboRosDiffDrive::UpdateOdometryEncoder()
{
double vl = joints_[LEFT].GetVelocity ( 0 );
double vr = joints_[RIGHT].GetVelocity ( 0 );
/*common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_odom_update_ ).Double();
*///greg
ros::Time current_time =getSimTime();
//double seconds_since_last_update = (current_time - last_odom_update_); // .Double(); //greg
double seconds_since_last_update = current_time.toSec() - last_odom_update_.toSec(); // .Double(); //greg
ROS_INFO_NAMED("interop", "UPDATEODOMETRY secsincelast:%f, cur:%u, lodom:%u",seconds_since_last_update, current_time.toSec(), last_odom_update_.toSec());
last_odom_update_ = current_time;
double b = wheel_separation_;
// Book: Sigwart 2011 Autonompus Mobile Robots page:337
double sl = vl * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update;
double theta = ( sl - sr ) /b;
double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) );
double dtheta = ( sl - sr ) /b;
pose_encoder_.x += dx;
pose_encoder_.y += dy;
pose_encoder_.theta += dtheta;
double w = dtheta/seconds_since_last_update;
double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update;
tf::Quaternion qt;
tf::Vector3 vt;
qt.setRPY ( 0,0,pose_encoder_.theta );
vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 );
odom_.pose.pose.position.x = vt.x();
odom_.pose.pose.position.y = vt.y();
odom_.pose.pose.position.z = vt.z();
odom_.pose.pose.orientation.x = qt.x();
odom_.pose.pose.orientation.y = qt.y();
odom_.pose.pose.orientation.z = qt.z();
odom_.pose.pose.orientation.w = qt.w();
odom_.twist.twist.angular.z = w;
odom_.twist.twist.linear.x = dx/seconds_since_last_update;
odom_.twist.twist.linear.y = dy/seconds_since_last_update;
}
void GazeboRosDiffDrive::Reset()
{
last_update_time_ = getSimTime(); // parent->GetWorld()->GetSimTime();
pose_encoder_.x = 0;
pose_encoder_.y = 0;
pose_encoder_.theta = 0;
x_ = 0;
rot_ = 0;
#ifdef USE_GAZEBO
#if GAZEBO_MAJOR_VERSION > 2
joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque );
joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque );
#else
joints_[LEFT].SetMaxForce ( 0, wheel_torque );
joints_[RIGHT].SetMaxForce ( 0, wheel_torque );
#endif
#endif
}
void SimulatorParameters::loadNumericParameters(GeomData *pGCData){
string str;
ifstream fid;
// read numeric parameters
string numeric(parametersPath + "/numeric.dat");
fid.open(numeric.c_str());
if ( !fid.is_open() ){
char msg[256]; sprintf(msg,"File: %s \ncould not be opened or it doesn't exist\n",numeric.c_str());
throw Exception(__LINE__,__FILE__,msg);
}
// start reading file
// -------------------------------------------------------------------------
setPositionToRead(fid,"type method ID right below:");
fid >> ellipticSolver;
setPositionToRead(fid,"type method ID right below:");
fid >> hyperbolicSolver;
setPositionToRead(fid,"Use high order approximation:");
fid >> str;
if (!str.compare("yes")){
setUseHOApproximation();
}
setPositionToRead(fid,"C.F.L (Courant-Friedilich-Lendroff):");
fid >> _CFL;
setPositionToRead(fid,"Discretization time on production well:");
fid >> _timeStepWell;
if (!str.compare("yes") && _CFL > .5)
throw Exception(__LINE__,__FILE__,"High order approximation cannot use CFL condition number greater than 0.5!\n");
setPositionToRead(fid,"mesh filename:");
fid >> prepfName;
setPositionToRead(fid,"export filename (to VTK format without extension .vtk):");
fid >> expofName;
string strYes;
setPositionToRead(fid,"Start simulation from <restart> file?");
getline(fid,strYes);
setPositionToRead(fid,"Dirichlet (Specify: flags and respective pressure values):");
mapFlag(fid,"dirichlet");
setPositionToRead(fid,"Injection Wells: 10 - 50");
getWells(fid);
setPositionToRead(fid,"Production Wells: 51 - 90");
getWells(fid);
setPositionToRead(fid,"Specify water saturation in injection wells:");
mapFlag(fid,"saturation");
setPositionToRead(fid,"water saturation at t=0:");
fid >> _Sw_initial;
setPositionToRead(fid,"simulation-time:");
fid >> _ST;
setPositionToRead(fid,"dvtol:");
fid >> _dvtol;
setPositionToRead(fid,"Reservoir Dimensions: L - H:");
fid >> reservoir_Length >> reservoir_Height;
pGCData->setReservoirHeight(reservoir_Height);
fid.close();
// RESTART: check if simulation starts from the beginning or from where it stopped.
string::size_type pos = strYes.find("yes",0);
if (pos != string::npos){
restart = true;
ifstream rfid;
// seek for the last VTK file generated
for (int i=0; i<=100; i++){
char strtmp[512]; sprintf(strtmp,"%s__0-of-1__step-%d.vtk",expofName.c_str(),i);
rfid.open(strtmp);
if (rfid.is_open()){
rfid.close();
}
else{
cout << "\nRestart:\n--------------------------------------------------------------------------------------\n";
if (!i){
throw Exception(__LINE__,__FILE__,"Any VTK file has been generated using the provided case file. Please, check carefully for mistypes\n");
}
lastpvi = i-1; // lastpvi=i is the VTK file that should be created by simulation was terminated. So the last is i-1.
PVI_cumulative = double(lastpvi/100.0);
char strtmp2[512]; sprintf(strtmp2,"%s__0-of-1__step-%d.vtk",expofName.c_str(),lastpvi);
restartFilename = strtmp2;
rfid.close();
cout << "\nSimulation will be resumed from VTK file:\n" << restartFilename << endl;
// set cumulative simulation time variable: cumTS = summation of all time steps (from the beginning to PVI i-1
string strline;
string data[8];
char strtmp4[512]; sprintf(strtmp4,"%s_simulation-monitor-%d.csv",expofName.c_str(),P_size());
rfid.open(strtmp4);
if (!rfid.is_open()){
throw Exception(__LINE__,__FILE__,"Simulation monitor file could not be opened.\n");
}
break;
}
}
this->setStepOutputFile(lastpvi+1);
cumTS = PVI_cumulative*this->getSimTime();
cout << "Simulation Time : " << getSimTime();
cout << "\nCumulative Simulation Time: " << cumTS << " [" << setprecision(2) << double(100.*cumTS/getSimTime()) << "% concluded]" << endl;
cout << "lastpvi= " << lastpvi << endl;
}
if (!P_pid()){
cout << "SIMULATION INFO:\n";
cout << "-------------------------------------------------------------------------------------------------------------------------";
cout << "\nPath and file name:\n";
cout << prepfName << "\n";
cout << "Output path and file name:\n";
cout << expofName << endl;
}
}
// Update the controller
void GazeboRosDiffDrive::UpdateChild() //this is Tick();
{
/* force reset SetParam("fmax") 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)
*/
#ifdef USE_GAZEBO
for ( int i = 0; i < 2; i++ ) {
#if GAZEBO_MAJOR_VERSION > 2
if ( fabs(wheel_torque -joints_[i]->GetParam ( "fmax", 0 )) > 1e-6 ) {
joints_[i]->SetParam ( "fmax", 0, wheel_torque );
#else
if ( fabs(wheel_torque -joints_[i]->GetMaxForce ( 0 )) > 1e-6 ) {
joints_[i]->SetMaxForce ( 0, wheel_torque );
#endif
}
}
#endif
if (odom_source_ == ENCODER)
{
UpdateOdometryEncoder();
// ROS_INFO_NAMED("interop", "jsize:%d",joints_.size());
}
// common::Time current_time = parent->GetWorld()->GetSimTime();
ros::Time current_time = getSimTime();
double seconds_since_last_update = (current_time - last_update_time_).toSec(); // .Double(); //greg
ROS_INFO_NAMED("interop", "UPDATE CHILD, since last update:%f ct:%f lu:%f", seconds_since_last_update, current_time.toSec() , last_update_time_.toSec());
if ( seconds_since_last_update > update_period_ )
{
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
// if ( publishWheelTF_ ) publishWheelTF(); //greg
// 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
#if GAZEBO_MAJOR_VERSION > 2
joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
#else
joints_[LEFT].SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT].SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
#ifdef WITH_LOGGING
ROS_INFO_NAMED("interop", "^actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
ROS_INFO_NAMED("interop", "^actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT], wheel_speed_[RIGHT]);
#endif
#endif
} 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 );
#ifdef WITH_LOGGING
ROS_INFO_NAMED("interop","*actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
ROS_INFO_NAMED("interop","*actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
#endif
#if GAZEBO_MAJOR_VERSION > 2
joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
#else
joints_[LEFT].SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT].SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
#endif
#ifdef WITH_LOGGING
ROS_INFO_NAMED("interop", "+actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
ROS_INFO_NAMED("interop", "+actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT], wheel_speed_[RIGHT]);
#endif
}
// last_update_time_+= common::Time ( update_period_ );//greg
last_update_time_ = ros::Time::now();
last_update_time_ = last_update_time_ + ros::Duration(update_period_); //is this right? seems to work but not tested with period > 0
}
}
// Finalize the controller
void GazeboRosDiffDrive::FiniChild()
{
ROS_INFO_NAMED("interop", "Finichild called");
cmd_vel_subscriber_.shutdown();
gazebo_ros_->shutdown();
alive_ = false;
queue_.clear();
queue_.disable();
// gazebo_ros_->shutdown();
callback_queue_thread_.join();
//new greag
// delete &joints_[0];
// delete &joints_[1];
joints_.pop_back();
joints_.pop_back();
joints_.empty();
ROS_INFO_NAMED("interop", "Finichild - callback_que_thread terminated, stats shutdown:%d", gazebo_ros_->ok());
}
