Project

General

Profile

Revision 066966cf

ID066966cfe3d2a047c4ee56a070af3080e30da10d
Parent f75a88be
Child 4a4f0c35

Added by tahm about 10 years ago

BuggyVis works

View differences:

buggyvis/src/buggy.cpp
63 63 
    //Services
64 64 
    set_pen_srv_ = nh_.advertiseService
65 65 
      ("set_pen", &Turtle::setPenCallback, this);
66 
    teleport_relative_srv_ = nh_.advertiseService
67 
      ("teleport_relative", &Turtle::teleportRelativeCallback, this);
68 
    teleport_absolute_srv_ = nh_.advertiseService
69 
      ("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);
66 
    //teleport_relative_srv_ = nh_.advertiseService
67 
    //  ("teleport_relative", &Turtle::teleportRelativeCallback, this);
68 
    //teleport_absolute_srv_ = nh_.advertiseService
69 
    //  ("teleport_absolute", &Turtle::teleportAbsoluteCallback, this);
70 70
71 71 
    meter_ = turtle_image_.height();
72 72 
    rotateImage();
......
75 75 
  void Turtle::poseCallback(const buggymsgs::PoseConstPtr& pcp)
76 76 
  {
77 77 
    last_command_time_ = ros::WallTime::now();
78 
    pose_ = pcp->pose;
78 
    pose_ = *pcp;
79 79 
  }
80 80
81 81 
  bool Turtle::setPenCallback(buggysim::SetPen::Request& req,
......
108 108 
   * TODO: convert this to take just an angle. And include drag.
109 109 
   *       and include acceleration/joule dump
110 110 
   */
111 
  bool Turtle::update(double dt,
112 
		      QPainter& path_painter,
111 
  bool Turtle::update(QPainter& path_painter,
113 112 
		      const QImage& path_image,
114 113 
		      qreal canvas_width,
115 114 
		      qreal canvas_height)
......
117 116 
    bool modified = false;
118 117 
    qreal old_orient = orient_;
119 118 

  		




  120
  
  
    
    // first process any teleportation requests, in order


  		




  121
  
  
    
    V_TeleportRequest::iterator it = teleport_requests_.begin();


  		




  122
  
  
    
    V_TeleportRequest::iterator end = teleport_requests_.end();


  		




  123
  
  
    
    for (; it != end; ++it)


  		




  124
  
  
    
      {


  		




  125
  
  
    
	const TeleportRequest& req = *it;


  		




  126
  
  
    

  		




  127
  
  
    
	QPointF old_pos = pos_;


  		




  128
  
  
    
	if (req.relative)


  		




  129
  
  
    
	  {


  		




  130
  
  
    
	    orient_ += req.theta;


  		




  131
  
  
    
	    pos_.rx() += std::sin(orient_ + PI/2.0) * req.linear;


  		




  132
  
  
    
	    pos_.ry() += std::cos(orient_ + PI/2.0) * req.linear;


  		




  133
  
  
    
	  }


  		




  134
  
  
    
	else


  		




  135
  
  
    
	  {


  		




  136
  
  
    
	    pos_.setX(req.pos.x());


  		




  137
  
  
    
	    pos_.setY(std::max(0.0, static_cast<double>(canvas_height - req.pos.y())));


  		




  138
  
  
    
	    orient_ = req.theta;


  		




  139
  
  
    
	  }


  		




  140
  
  
    

  		




  141
  
  
    
	path_painter.setPen(pen_);


  		




  142
  
  
    
	path_painter.drawLine(pos_ * meter_, old_pos * meter_);


  		




  143
  
  
    
	modified = true;


  		




  144
  
  
    
      }


  		




  145
  
  
    

  		




  146
  119
  
    
    teleport_requests_.clear();


  		




  147
  120
  
    

  		




  148
  
  
    
    // Stop moving after 1 second since-last command.


  		




  149
  
  
    

  		




  150
  121
  
    
    // Update my position


  		




  151
  122
  
    
    QPointF old_pos = pos_;


  		




  
  123
  
    
    double dt = (ros::WallTime::now() - last_command_time_).toSec();


  		




  152
  124
  
    

  		




  153
  
  
    
    // TODO: what direction does orientation start at 0?


  		




  154
  
  
    
    // TODO: orient might have to become negative


  		




  155
  
  
    
    orient_ = std::fmod(orient_ - (turnAngle_ * dt), 2*PI);


  		




  156
  
  
    
    if (orient_ < 0.0) {


  		




  157
  
  
    
      orient_ += 2*PI;


  		




  158
  
  
    
    }


  		




  
  125
  
    
    //orient_ = std::fmod(orient_ - (turnAngle_ * dt), 2*PI);


  		




  
  126
  
    
    //if (orient_ < 0.0) {


  		




  
  127
  
    
    //  orient_ += 2*PI;


  		




  
  128
  
    
    //}


  		




  159
  129
  
    
    // TODO: why add pi/2? is this just because they got the 


  		




  160
  130
  
    
    //       sin/cos mixed up?


  		




  161
  
  
    
    pos_.rx() += std::cos(orient_) * lin_vel_ * dt;


  		




  162
  
  
    
    pos_.ry() -= std::sin(orient_) * lin_vel_ * dt;


  		




  
  131
  
    
    //pos_.rx() += std::cos(orient_) * lin_vel_ * dt;


  		




  
  132
  
    
    //pos_.ry() -= std::sin(orient_) * lin_vel_ * dt;


  		




  
  133
  
    

  		




  
  134
  
    
    pos_.rx() = pose_.x;


  		




  
  135
  
    
    pos_.ry() = pose_.y;


  		




  163
  136
  
    

  		




  164
  137
  
    
    // Clamp to screen size


  		




  165
  138
  
    
    // TODO: change to use another image, to detect walls. 


  		




  ......




  175
  148
  
    
		       static_cast<double>(canvas_height)));


  		




  176
  149
  
    

  		




  177
  150
  
    
    /** END POSITION UPDATE */


  		




  178
  
  
    
    buggymsgs::Pose p;


  		




  179
  
  
    
    p.x = pos_.x();


  		




  180
  
  
    
    p.y = canvas_height - pos_.y();


  		




  181
  
  
    
    p.angle = orient_;


  		




  182
  
  
    
    p.linear_velocity = lin_vel_;


  		




  183
  
  
    
    pose_pub_.publish(p);


  		




  
  151
  
    
    //buggymsgs::Pose p;


  		




  
  152
  
    
    //p.x = pos_.x();


  		




  
  153
  
    
    //p.y = canvas_height - pos_.y();


  		




  
  154
  
    
    //p.angle = orient_;


  		




  
  155
  
    
    //p.linear_velocity = lin_vel_;


  		




  
  156
  
    
    //pose_pub_.publish(p);


  		




  184
  157
  
    

  		




  185
  158
  
    
    ROS_DEBUG("[%s]: pos_x: %f pos_y: %f theta: %f", 


  		




  186
  159
  
    
	      nh_.getNamespace().c_str(), 


  		












Also available in:  Unified diff