void FDStream::waitWrite(base::Time const& timeout)
{
fd_set set;
FD_ZERO(&set);
FD_SET(m_fd, &set);
timeval timeout_spec = { static_cast<time_t>(timeout.toSeconds()), timeout.toMicroseconds() % 1000000 };
int ret = select(m_fd + 1, NULL, &set, NULL, &timeout_spec);
if (ret < 0 && errno != EINTR)
throw UnixError("waitWrite(): error in select()");
else if (ret == 0)
throw TimeoutError(TimeoutError::NONE, "waitWrite(): timeout");
}
bool TouchPlatformFile(PlatformFile file, const base::Time& last_access_time,
const base::Time& last_modified_time)
{
ThreadRestrictions::AssertIOAllowed();
if(file == kInvalidPlatformFileValue)
{
return false;
}
FILETIME last_access_filetime = last_access_time.ToFileTime();
FILETIME last_modified_filetime = last_modified_time.ToFileTime();
return (SetFileTime(file, NULL, &last_access_filetime,
&last_modified_filetime) != 0);
}
bool Task::perception_state_machine(const base::Time& ts)
{
if(orientation_sample_recieved){ //we have a valid orientation
if(current_depth < _minimum_depth.get()){ //we have a valid depth
if(last_motion.isNull() || ts.toSeconds() - last_motion.toSeconds() > _reset_timeout.get()) //Joint timeout
changeState(NO_JOINTS_NO_DVL);
else if(last_hough.isNull() || ts.toSeconds() - last_hough.toSeconds() > _hough_timeout.get()){ //Hough timeout
changeState(NO_HOUGH);
//get timedifference to last hpugh timeout, and intersper random particles after a given time
if(ts.toSeconds() - last_hough.toSeconds() > _hough_timeout.get()){
if(last_hough_timeout.isNull()){
last_hough_timeout = ts;
}
else if(ts.toSeconds() - last_hough_timeout.toSeconds() > _hough_timeout.get()){
last_hough_timeout = ts;
localizer->interspersal(base::samples::RigidBodyState(), *map, _hough_timeout_interspersal.get(), true, true);
}
}
}
else //Everything is fine :-)
changeState(LOCALIZING);
last_perception = ts;
if(number_rejected_samples < static_cast<unsigned>(_init_sample_rejection.value())) {
number_rejected_samples++;
return false;
}
return true;
}else{ //Invalid depth
changeState(ABOVE_SURFACE);
}
}
else{ //Invalid or no orientation
changeState(NO_ORIENTATION);
}
return false;
}
void PortStream::waitRead(base::Time const& timeout)
{
if (!mPacketRead.data.empty())
return;
uint64_t sleep_time;
int count;
uint64_t full_timeout = timeout.toMicroseconds();
if (full_timeout > 10000)
{
sleep_time = 10000;
count = (full_timeout + sleep_time - 1) / sleep_time;
}
else
{
count = 1;
sleep_time = full_timeout;
}
for (int i = 0; i < count; ++i)
{
if (mIn.read(mPacketRead, false) == RTT::NewData)
return;
usleep(sleep_time);
}
throw TimeoutError(TimeoutError::NONE, "waitRead(): timeout");
}
void calculateSamples(int nr)
{
base::Time sampleLatencyMaxNoise;
if (nr > 0)
sampleLatencyMaxNoise = this->sampleLatencyMaxNoise;
else
sampleLatencyMaxNoise = realPeriod * 0.09;
base::Time sampleLatencyNoise = base::Time::fromSeconds(drand48() * sampleLatencyMaxNoise.toSeconds());
base::Time hwTimeNoise(base::Time::fromSeconds(drand48() * hwTimeMaxNoise.toSeconds()));
actualPeriod = realPeriod + periodDrift * nr;
realTime = baseTime + realPeriod * nr + periodDrift * nr * (nr + 1) / 2;
sampleTime = realTime + sampleLatency + sampleLatencyNoise;
hwTime = realTime + hwTimeNoise;
}
void Task::buoy_samplesCallback(const base::Time& ts, const avalon::feature::Buoy& buoy){
if(!_use_slam.get() ){
if(lastRBS.cov_position(0,0) <= _position_covariance_threshold.get() && lastRBS.cov_position(1,1) <= _position_covariance_threshold.get() ){
if(buoy.color == avalon::feature::NO_BUOY){
return;
}
BuoyColor bc;
if(buoy.color == avalon::feature::WHITE){
bc = WHITE;
}else if(buoy.color == avalon::feature::ORANGE){
bc = ORANGE;
}
else if(buoy.color == avalon::feature::UNKNOWN){
bc = UNKNOWN;
}
base::Vector3d buoyPose = lastRBS.position + (lastRBS.orientation * config.buoyCamPosition);
if(grid_map->setBuoy(buoyPose.x(), buoyPose.y(), bc , buoy.probability, true)){
std::cout << "BOJE!" << "(" << ts.toString() << "," << buoyPose.x() << "," << buoyPose.y() << "," << buoyPose.z() << ",FOUND_BUOY)" << std::endl;
}
}
}
//double effective_sample_size = localizer->observe(buoy, *map, _buoy_importance.value());
}
void addResultToPlot(base::Time estimatedTime, base::Time estimatedPeriod)
{
static base::Time lastEstimatedTime = estimatedTime;
static base::Time lastRealTime = realTime;
if(debugFile.is_open())
{
debugFile << (hwTime - realTime).toSeconds() / actualPeriod.toSeconds()
<< " " << (sampleTime - realTime - sampleLatency).toSeconds() / actualPeriod.toSeconds()
<< " " << (estimatedTime - realTime).toSeconds() / actualPeriod.toSeconds()
<< " " << (estimatedPeriod - actualPeriod).toSeconds() / actualPeriod.toSeconds()
<< " " << (estimatedTime - lastEstimatedTime).toSeconds()
<< " " << (realTime - lastRealTime).toSeconds()
<< " " << actualPeriod.toSeconds()
<< std::endl;
}
lastEstimatedTime = estimatedTime;
lastRealTime = realTime;
}
inline void step_dt(const base::Time& dt){
if(mode == base::JointState::POSITION)
{
j_state.position = j_setpoint.position;
j_state.speed = j_setpoint.speed;
j_state.effort = j_setpoint.effort;
trunc_to_limit(base::JointState::POSITION);
trunc_to_limit(base::JointState::SPEED);
trunc_to_limit(base::JointState::EFFORT);
}
else if(mode == base::JointState::SPEED)
{
j_state.speed = j_setpoint.speed;
j_state.effort = j_setpoint.effort;
trunc_to_limit(base::JointState::SPEED);
trunc_to_limit(base::JointState::EFFORT);
j_state.position += j_setpoint.speed*dt.toSeconds();
trunc_to_limit(base::JointState::POSITION);
}
else if(mode == base::JointState::EFFORT)
{
j_state.effort = j_setpoint.effort;
trunc_to_limit(base::JointState::EFFORT);
j_state.speed += j_setpoint.effort*dt.toSeconds();
j_state.position += j_setpoint.speed*dt.toSeconds();
trunc_to_limit(base::JointState::POSITION);
trunc_to_limit(base::JointState::SPEED);
}
perturb(dt.toSeconds());
}
void Task::thruster_samplesCallback(const base::Time& ts, const base::samples::Joints& status)
{
//base::Time temp = base::Time::now();
if(last_speed_time.isNull() || ts.toSeconds() - last_speed_time.toSeconds() > _speed_samples_timeout.get() ){
base::samples::Joints j = status;
last_motion = ts;
//If we have no 6 thruster, fill up the joints with zeros
/*if(j.size() < 6){
unsigned int size = j.size();
j.elements.resize(6);
j.names.resize(6);
for(; size < 6; size++){
j.elements[size].raw = 0.0;
}
}*/
if(status.hasNames()){
for(unsigned int i = 0; i < status.size() && i < config.joint_names.size(); i++){
try{
j.elements[i] = status[config.joint_names[i]];
}
catch(...){
}
}
}
if(orientation_sample_recieved){
localizer->update_dead_reckoning(j);
localizer->update(j, *map);
}else{
changeState(NO_ORIENTATION);
}
}
//std::cout << "Calc time thruster samples: " << base::Time::now().toSeconds() - temp.toSeconds() << std::endl;
}
void ros_convertions::toROS( ros::Time& ros, ::base::Time const& value )
{
ros.fromNSec(value.toMicroseconds() * 1000);
}
void checkResult(base::Time estimatedTime, base::Time estimatedPeriod)
{
addResultToPlot(estimatedTime, estimatedPeriod);
BOOST_CHECK_SMALL((estimatedTime - realTime).toSeconds(), actualPeriod.toSeconds() / 10);
}
void SoftTurnController::updateHook()
{
SoftTurnControllerBase::updateHook();
double pointTurnLimit = 0.1;
double stepLimit = 0.01;
static double lastSpeed = 0;
static base::Time changeTime;
base::actuators::Status status;
if(_status.readNewest(status, true) == RTT::NoData)
return;
// This is the user's command
base::commands::Motion2D cmd_in;
if (_motion_command.readNewest(cmd_in, true) == RTT::NoData)
{
cmd_in.translation = 0;
cmd_in.rotation = 0;
}
//let's allways turn
if(true || fabs(cmd_in.rotation) > pointTurnLimit)
{
//hack
cmd_in.rotation = 1.0;
cmd_in.rotation = turnSpeed * fabs(cmd_in.rotation) / cmd_in.rotation;
static bool switchState = false;
for(int i = 0; i < 4; i++)
{
const double diff = fabs(startStatus.states[i].positionExtern - status.states[i].positionExtern);
//we assume zero slip
if(diff * _wheel_radius.get() > _turnVariance.get() )
{
//we moved the maximum variance in one direction, reverse
switchState = true;
break;
}
}
if(!switchState)
{
switch(state)
{
case FORWARD:
cmd_in.translation = translationalSpeed;
break;
case BACKWARD:
cmd_in.translation = -translationalSpeed;
break;
}
}
else
{
if(changeTime.isNull())
changeTime = base::Time::now();
if(base::Time::now() - changeTime > base::Time::fromSeconds(0.5) )
{
startStatus = status;
if(state == FORWARD)
state = BACKWARD;
else
state = FORWARD;
std::cout << "Switched State" << std::endl;
changeTime = base::Time();
switchState = false;
}
}
}
double newSpeed = 0;
if(cmd_in.translation < 0)
{
newSpeed = lastSpeed - stepLimit;
if(newSpeed < cmd_in.translation)
newSpeed = cmd_in.translation;
}
else
{
newSpeed = lastSpeed + stepLimit;
if(newSpeed > cmd_in.translation)
newSpeed = cmd_in.translation;
}
lastSpeed = newSpeed;
cmd_in.translation = newSpeed;
std::cout << "cmd_in.translation " << cmd_in.translation << "cmd_in.rotation " << cmd_in.rotation << std::endl;
double fwd_velocity = cmd_in.translation / _wheel_radius.get();
double differential = cmd_in.rotation * _track_width.get() / _wheel_radius.get();
double leftSpeed;
double rightSpeed;
m_cmd.mode[0] = m_cmd.mode[1] =
m_cmd.mode[2] = m_cmd.mode[3] = base::actuators::DM_SPEED;
m_cmd.target[base::actuators::WHEEL4_FRONT_LEFT] = fwd_velocity - differential;
m_cmd.target[base::actuators::WHEEL4_REAR_LEFT] = fwd_velocity - differential;
m_cmd.target[base::actuators::WHEEL4_FRONT_RIGHT] = fwd_velocity + differential;
m_cmd.target[base::actuators::WHEEL4_REAR_RIGHT] = fwd_velocity + differential;
m_cmd.time = base::Time::now();
_simple_command.write(m_cmd);
}
