void Connection::SendToAll(u8 channelnum, SharedBuffer<u8> data, bool reliable)
{
assert(channelnum < CHANNEL_COUNT);
ConnectionCommand c;
c.sendToAll(channelnum, data, reliable);
putCommand(c);
}
int DynamicWindowController::Shutdown () {
StopThread();
this->laser_device->Unsubscribe(this->InQueue);
this->position_device->Unsubscribe(this->InQueue);
putCommand(0,0);
return 0;
}
void DynamicWindowController::Main() {
struct timeval past;
gettimeofday(&past,NULL);
while(true) {
Wait();
pthread_testcancel();
ProcessMessages();
double dist;
double desired_theta;
if(!this->m_active_goal) {
//backward
if (m_own_cmd.vel.px <= 0) {
putCommand(m_own_cmd.vel.px,m_own_cmd.vel.pa);
continue;
}
dist = 2.0*m_dist_done;
desired_theta = m_own_cmd.vel.pa;
//std::cout<<"Received: "<<m_own_cmd.vel.px<<"\n";
dw.setVmax(m_own_cmd.vel.px);
}
else {
dw.setVmax(m_vmax);
dist = hypot(m_x_goal-m_x_robot, m_y_goal - m_y_robot);
double theta_goal = atan2(m_y_goal - m_y_robot,m_x_goal - m_x_robot);
DynamicWindow::fix_angle(theta_goal);
desired_theta = theta_goal - m_theta_robot;
DynamicWindow::fix_angle(desired_theta);
}
bool turn_in_place = false;
// std::cout<<"Distance to goal: "<<dist<<"\n";
// std::cout<<"Desired theta: "<<DynamicWindow::rad2deg(desired_theta)<<"\n";
if (dist <= m_dist_done) {
if (fabs(m_theta_robot - m_theta_goal) < DynamicWindow::deg2rad(m_theta_done)) {
std::cout<<"Goal reached!\n";
m_active_goal = false;
bzero(&m_own_cmd,sizeof(m_own_cmd));
putCommand(0,0);
continue;
}
else {
std::cout<<"Turning in place\n";
turn_in_place = true;
}
}
struct timeval present;
gettimeofday(&present,NULL);
double elapsed = present.tv_sec -past.tv_sec + double(present.tv_usec - past.tv_usec)/1.0e06;
if (elapsed < 0.8*m_tcmax) {
//std::cout<<"INFO: "<<elapsed<<"\n";
continue;
}
if (elapsed > m_tcmax) {
//PLAYER_WARN1("WARNING, TAKING TOO LONG: %f",elapsed);
}
gettimeofday(&past,NULL);
double v,w;
if (turn_in_place) {
double tmp_theta = m_theta_goal - m_theta_robot;
DynamicWindow::fix_angle(tmp_theta);
dw.motion_model_sampling_time(m_tcmax-0.02,m_v_robot,m_w_robot,tmp_theta,m_x_obstacle,m_y_obstacle,v,w);
v=0;
}
else {
dw.motion_model_sampling_time(m_tcmax-0.02,m_v_robot,m_w_robot,desired_theta,m_x_obstacle,m_y_obstacle,v,w);
}
if ( (m_active_goal) && (!turn_in_place) && (v<=0.03) ){
w = M_PI/3.0*(desired_theta>=0?1:-1);
v = -0.3;
}
putCommand(v,w);
}
}
void Connection::DisconnectPeer(u16 peer_id)
{
ConnectionCommand discon;
discon.disconnect_peer(peer_id);
putCommand(discon);
}
void Connection::DeletePeer(u16 peer_id)
{
ConnectionCommand c;
c.deletePeer(peer_id);
putCommand(c);
}
void Connection::Disconnect()
{
ConnectionCommand c;
c.disconnect();
putCommand(c);
}
void Connection::Connect(Address address)
{
ConnectionCommand c;
c.connect(address);
putCommand(c);
}
void Connection::Serve(Address bind_address)
{
ConnectionCommand c;
c.serve(bind_address);
putCommand(c);
}
