void laserCallBack(const sensor_msgs::LaserScan::ConstPtr& laser){
if(odomVal){
float xx, yy, yaw, optYaw, turnDir;
int obstacle = 0;
geometry_msgs::Twist msg;
xx = (odomVal -> pose).pose.position.x;
yy = (odomVal -> pose).pose.position.y;
yaw = tf::getYaw((odomVal -> pose).pose.orientation);
optYaw = optAngle(xx, yy);
if (!onSink(xx,yy)){
if (nearObject(laser)){ // Obstacle
avoidObstacle = true;
turnDir = reorient(laser);
msg.angular.z = turnAngle;
blockPath(laser, yaw, optYaw);
ROS_INFO("[Avoiding Obstacle] yaw: %.2f, avoid: %i\n", yaw, avoidObstacle);
} else if ((fabs(yaw-optYaw) > angErr) && !avoidObstacle){ // Not facing sink
msg.angular.z = turnAngle;
ROS_INFO("[Current Location] (x,y): (%.2f, %.2f)\n", xx, yy);
ROS_INFO("[Adjust Direction] yaw: %.2f opt: %.2f \n", yaw, optYaw);
} else { // Move robot
msg.linear.x = moveRate;
ROS_INFO("[Moving...]\n");
avoidObstacle = blockPath(laser, yaw, optYaw);
if(!avoidObstacle)
ROS_INFO("[Moving towards destination!!!]\n");
}
cmd_velPub.publish(msg);
}
}
}
bool Walkway::nearCoin(GLfloat x, GLfloat y, GLfloat z) {
return nearObject(x, y, z, coinDisks);
}
bool Walkway::nearEnergy(GLfloat x, GLfloat y, GLfloat z) {
return nearObject(x, y, z, energyBalls);
}