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