wavefront.cc

机器人仿真软件

  return 0;
}

void
Wavefront::ProcessCommand(player_planner_cmd_t* cmd)
{
  double new_x, new_y, new_a;
  //double eps = 1e-3;

  new_x = cmd->goal.px;
  new_y = cmd->goal.py;
  new_a = cmd->goal.pa;

#if 0
  if((fabs(new_x - this->target_x) > eps) ||
     (fabs(new_y - this->target_y) > eps) ||
     (fabs(this->angle_diff(new_a,this->target_a)) > eps))
  {
#endif
    this->target_x = new_x;
    this->target_y = new_y;
    this->target_a = new_a;
    printf("new goal: %f, %f, %f\n", target_x, target_y, target_a);
    this->new_goal = true;
    this->atgoal = false;
#if 0
  }
#endif
}

void
Wavefront::ProcessLocalizeData(player_position2d_data_t* data)
{
  this->localize_x = data->pos.px;
  this->localize_y = data->pos.py;
  this->localize_a = data->pos.pa;
}

void
Wavefront::ProcessPositionData(player_position2d_data_t* data)
{
  this->position_x = data->pos.px;
  this->position_y = data->pos.py;
  this->position_a = data->pos.pa;
}

void
Wavefront::ProcessMapInfo(player_map_info_t* info)
{
  // Got new map info pushed to us.  We'll save this info and get the new
  // map.
  this->plan->scale = info->scale;
  this->plan->size_x = info->width;
  this->plan->size_y = info->height;
  this->plan->origin_x = info->origin.px;
  this->plan->origin_y = info->origin.py;
  // Set the bounds to search the whole grid.
  plan_set_bounds(this->plan,
                  0, 0, this->plan->size_x - 1, this->plan->size_y - 1);

  // Now get the map data, possibly in separate tiles.
  if(this->GetMap(true) < 0)
  {
    this->have_map = false;
    this->StopPosition();
  }
  else
  {
    this->have_map = true;
    this->new_map = true;
    // force replanning
    if(this->curr_waypoint >= 0)
      this->new_goal = true;
  }
}

void
Wavefront::PutPlannerData()
{
  player_planner_data_t data;

  memset(&data,0,sizeof(data));

  if(this->waypoint_count > 0)
    data.valid = 1;
  else
    data.valid = 0;

  if((this->waypoint_count > 0) && (this->curr_waypoint < 0))
    data.done = 1;
  else
    data.done = 0;

  // put the current localize pose
  data.pos.px = this->localize_x;
  data.pos.py = this->localize_y;
  data.pos.pa = this->localize_a;

  data.goal.px = this->target_x;
  data.goal.py = this->target_y;
  data.goal.pa = this->target_a;

  if(data.valid && !data.done)
  {
    data.waypoint.px = this->waypoint_x;
    data.waypoint.py = this->waypoint_y;
    data.waypoint.pa = this->waypoint_a;

    data.waypoint_idx = this->curr_waypoint;
    data.waypoints_count = this->waypoint_count;
  }

  this->Publish(this->device_addr, NULL,
                PLAYER_MSGTYPE_DATA,
                PLAYER_PLANNER_DATA_STATE,
                (void*)&data,sizeof(data),NULL);
}

void
Wavefront::PutPositionCommand(double x, double y, double a, unsigned char type)
{
  player_position2d_cmd_vel_t vel_cmd;
  player_position2d_cmd_pos_t pos_cmd;

  memset(&vel_cmd,0,sizeof(vel_cmd));
  memset(&pos_cmd,0,sizeof(pos_cmd));

  if(type)
  {
    // position control
    pos_cmd.pos.px = x;
    pos_cmd.pos.py = y;
    pos_cmd.pos.pa = a;
    pos_cmd.state=1;
    this->position->PutMsg(this->InQueue,
                         PLAYER_MSGTYPE_CMD,
                         PLAYER_POSITION2D_CMD_POS,
                         (void*)&pos_cmd,sizeof(pos_cmd),NULL);
  }
  else
  {
    // velocity control (used to stop the robot)
    vel_cmd.vel.px = x;
    vel_cmd.vel.py = y;
    vel_cmd.vel.pa = a;
    vel_cmd.state=1;
    this->position->PutMsg(this->InQueue,
                         PLAYER_MSGTYPE_CMD,
                         PLAYER_POSITION2D_CMD_VEL,
                         (void*)&vel_cmd,sizeof(vel_cmd),NULL);
  }

}

void
Wavefront::LocalizeToPosition(double* px, double* py, double* pa,
                              double lx, double ly, double la)
{
  double offset_x, offset_y, offset_a;
  double lx_rot, ly_rot;

  offset_a = this->angle_diff(this->position_a,this->localize_a);
  lx_rot = this->localize_x * cos(offset_a) - this->localize_y * sin(offset_a);
  ly_rot = this->localize_x * sin(offset_a) + this->localize_y * cos(offset_a);

  offset_x = this->position_x - lx_rot;
  offset_y = this->position_y - ly_rot;

  //printf("offset: %f, %f, %f\n", offset_x, offset_y, RTOD(offset_a));

  *px = lx * cos(offset_a) - ly * sin(offset_a) + offset_x;
  *py = lx * sin(offset_a) + ly * cos(offset_a) + offset_y;
  *pa = la + offset_a;
}

void
Wavefront::StopPosition()
{
  if(!this->stopped)
  {
    //puts("stopping robot");
    PutPositionCommand(0.0,0.0,0.0,0);
    this->stopped = true;
  }
}

void
Wavefront::SetWaypoint(double wx, double wy, double wa)
{
  double wx_odom, wy_odom, wa_odom;

  // transform to odometric frame
  LocalizeToPosition(&wx_odom, &wy_odom, &wa_odom, wx, wy, wa);

  // hand down waypoint
  //printf("sending waypoint: %.3f %.3f %.3f\n",
         //wx_odom, wy_odom, RTOD(wa_odom));
  PutPositionCommand(wx_odom, wy_odom, wa_odom,1);

  // cache this waypoint, odometric coords
  this->waypoint_odom_x = wx_odom;
  this->waypoint_odom_y = wy_odom;
  this->waypoint_odom_a = wa_odom;

  this->stopped = false;
}


////////////////////////////////////////////////////////////////////////////////
// Main function for device thread
void Wavefront::Main()
{
  double dist, angle;
  double t;
  double last_replan_lx=0.0, last_replan_ly=0.0;
  double last_replan_time = 0.0;
  double last_publish_time = 0.0;
  double replan_timediff, replan_dist;
  bool rotate_waypoint=false;
  bool replan;

  pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED,NULL);

  // block until we get initial data from underlying devices
  // TODO
  //this->position->Wait();
  this->StopPosition();

  for(;;)
  {
    pthread_testcancel();

    ProcessMessages();

    if(!this->have_map && !this->new_map_available)
    {
      usleep(CYCLE_TIME_US);
      continue;
    }

    GlobalTime->GetTimeDouble(&t);

    if((t - last_publish_time) > 0.25)
    {
      last_publish_time = t;
      this->PutPlannerData();
    }

    // Is it time to replan?
    replan_timediff = t - last_replan_time;
    replan_dist = sqrt(((this->localize_x - last_replan_lx) *
                        (this->localize_x - last_replan_lx)) +
                       ((this->localize_y - last_replan_ly) *
                        (this->localize_y - last_replan_ly)));
    replan = (this->replan_dist_thresh >= 0.0) &&
            (replan_dist > this->replan_dist_thresh) &&
            (this->replan_min_time >= 0.0) &&
            (replan_timediff > this->replan_min_time) &&
            !this->atgoal;

    // Did we get a new goal, or is it time to replan?
    if(this->new_goal || replan)
    {
#if 0
      // Should we get a new map?
      if(this->new_map_available)
      {
        this->new_map_available = false;

        if(this->GetMapInfo(true) < 0)
          PLAYER_WARN("failed to get new map info");
        else
        {
          if(this->GetMap(true) < 0)
            PLAYER_WARN("failed to get new map data");
          else
          {
            this->new_map = true;
            this->have_map = true;
          }
        }
      }

      // We need to recompute the C-space if the map changed, or if the
      // goal or robot pose lie outside the bounds of the area we last
      // searched.
      if(this->new_map ||
         !plan_check_inbounds(plan,this->localize_x,this->localize_y) ||
          !plan_check_inbounds(plan,this->target_x,this->target_y))
      {
        // Unfortunately, this computation can take a while (e.g., 1-2
        // seconds).  So we'll stop the robot while it thinks.
        this->StopPosition();

        // Set the bounds to search only an axis-aligned bounding box
        // around the robot and the goal.
        plan_set_bbox(this->plan, 1.0, 3.0,
                      this->localize_x, this->localize_y,
                      this->target_x, this->target_y);

        struct timeval t0, t1;
        gettimeofday(&t0, NULL);
        plan_update_cspace(this->plan,this->cspace_fname);
        gettimeofday(&t1, NULL);
        printf("time to update: %f\n",
               (t1.tv_sec + t1.tv_usec/1e6) -
               (t0.tv_sec + t0.tv_usec/1e6));
        this->new_map = false;
      }
#endif

      // compute costs to the new goal
      plan_update_plan(this->plan, this->target_x, this->target_y);

      // compute a path to the goal from the current position
      plan_update_waypoints(this->plan, this->localize_x, this->localize_y);

#if 0
      if(this->plan->waypoint_count == 0)
      {
        // No path.  If we only searched a bounding box last time, grow the
        // bounds to encompass the whole map and try again.
        if((plan->min_x > 0) || (plan->max_x < (plan->size_x - 1)) ||
           (plan->min_y > 0) || (plan->max_y < (plan->size_y - 1)))
        {
          // Unfortunately, this computation can take a while (e.g., 1-2
          // seconds).  So we'll stop the robot while it thinks.
          this->StopPosition();

          // Set the bounds to search the whole grid.
          plan_set_bounds(this->plan, 0, 0,
                          this->plan->size_x - 1,
                          this->plan->size_y - 1);

          struct timeval t0, t1;
          gettimeofday(&t0, NULL);
          plan_update_cspace(this->plan,this->cspace_fname);
          gettimeofday(&t1, NULL);
          printf("time to update: %f\n",
               (t1.tv_sec + t1.tv_usec/1e6) -
               (t0.tv_sec + t0.tv_usec/1e6));

          // compute costs to the new goal
          plan_update_plan(this->plan, this->target_x, this->target_y);

          // compute a path to the goal from the current position
          plan_update_waypoints(this->plan,
                                this->localize_x,
                                this->localize_y);
        }
      }
#endif

      if(this->plan->waypoint_count == 0)
      {
        fprintf(stderr, "Wavefront (port %d):\n  "
                "No path from (%.3lf,%.3lf,%.3lf) to (%.3lf,%.3lf,%.3lf)\n",
                this->device_addr.robot,
                this->localize_x,
                this->localize_y,
                RTOD(this->localize_a),
                this->target_x,
                this->target_y,
                RTOD(this->target_a));
        // Only fail here if this is our first try at making a plan;
        // if we're replanning and don't find a path then we'll just stick
        // with the old plan.
        if(this->curr_waypoint < 0)
        {
          //this->curr_waypoint = -1;
          this->new_goal=false;
          this->waypoint_count = 0;
        }
      }
      else if(this->plan->waypoint_count > PLAYER_PLANNER_MAX_WAYPOINTS)
      {
        PLAYER_WARN("Plan exceeds maximum number of waypoints!");
        this->waypoint_count = PLAYER_PLANNER_MAX_WAYPOINTS;
      }
      else
        this->waypoint_count = this->plan->waypoint_count;

      if(this->plan->waypoint_count > 0)
      {
        for(int i=0;i<this->plan->waypoint_count;i++)
        {
          double wx, wy;
          plan_convert_waypoint(this->plan,
                                this->plan->waypoints[i],
                                &wx, &wy);
          this->waypoints[i][0] = wx;
          this->waypoints[i][1] = wy;
        }

        this->curr_waypoint = 0;
        this->new_goal = true;
      }
      last_replan_time = t;
      last_replan_lx = this->localize_x;
      last_replan_ly = this->localize_y;
    }

    if(!this->enable)
    {
      this->StopPosition();
      usleep(CYCLE_TIME_US);
      continue;
    }

    bool going_for_target = (this->curr_waypoint == this->plan->waypoint_count);
    dist = sqrt(((this->localize_x - this->target_x) *
                 (this->localize_x - this->target_x)) +
                ((this->localize_y - this->target_y) *
                 (this->localize_y - this->target_y)));
    // Note that we compare the current heading and waypoint heading in the
    // *odometric* frame.   We do this because comparing the current
    // heading and waypoint heading in the localization frame is unreliable
    // when making small adjustments to achieve a desired heading (i.e., the
    // robot gets there and VFH stops, but here we don't realize we're done
    // because the localization heading hasn't changed sufficiently).
    angle = fabs(this->angle_diff(this->waypoint_odom_a,this->position_a));
    if(going_for_target && dist < this->dist_eps && angle < this->ang_eps)
    {
      // we're at the final target, so stop
      StopPosition();
      this->curr_waypoint = -1;
      this->new_goal = false;
      this->atgoal = true;
    }
    else if(this->curr_waypoint < 0)
    {
      // no more waypoints, so stop
      StopPosition();
    }
    else
    {
      // are we there yet?  ignore angle, cause this is just a waypoint
      dist = sqrt(((this->localize_x - this->waypoint_x) *
                   (this->localize_x - this->waypoint_x)) +
                  ((this->localize_y - this->waypoint_y) *
                   (this->localize_y - this->waypoint_y)));
      // Note that we compare the current heading and waypoint heading in the
      // *odometric* frame.   We do this because comparing the current
      // heading and waypoint heading in the localization frame is unreliable