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);
}
}
}
int findWalk(){
if(A[Z] != dimensions[H]){
reorient();
}
if(B[X] == 0){ // B is on face 1.
// The distance is given by (B[Y] - A[Y])^2 + (B[Z] + A[X])^2
int dy = B[Y] - A[Y],
dx = B[Z] + A[X];
return (dy*dy) + (dx*dx);
} else if(B[X] == dimensions[L]){
}
}
void Pin::rebuild()
{
if (pinname.hidden)
pinname.hide();
else
pinname.show();
if (pinnumber.hidden)
pinnumber.hide();
else
pinnumber.show();
rebuildShape();
setPath(buildPath());
reorient();
updatePostition();
}
QVariant Pin::itemChange(GraphicsItemChange change, const QVariant &value)
{
// Note that pins are only movable within the symbol editor.
PageScene *pageScene = dynamic_cast<PageScene *>(scene());
if (ItemPositionChange == change && pageScene) {
QPointF p = value.toPointF() + pageScene->reparentOffset(this); // p is now the position relative to the current parent.
SymbolEditor *se = dynamic_cast<SymbolEditor *>(pageScene->parent());
if (se) {
QGraphicsItem *anchor = se->closestPinAnchor(parentItem()->mapToScene(p), this);
if (parentItem() != anchor) {
pageScene->reparentWhileDragging(this, anchor);
p = value.toPointF() + pageScene->reparentOffset(this);
setData(FreeSCH::SectionChanged, true);
}
if (QGraphicsLineItem::Type == anchor->type()) {
p.setX(anchor->pos().x());
if (position.side() != PinPosition::Right) {
position.setSide(PinPosition::Right);
attributes.setValue("side", PinPosition::sideNames.at(PinPosition::Right));
reorient();
}
} else if (Section::Type == anchor->type()) {
Section *section = qgraphicsitem_cast<Section *>(anchor);
if (section) {
QRectF r = QRectF(QPointF(0,0), section->rect().size());
PinPosition::PinSide newside = PinPosition::sideIndex(r, p);
switch (newside) {
case PinPosition::Right:
p.setX(r.right());
break;
case PinPosition::Bottom:
p.setY(r.bottom());
break;
case PinPosition::Left:
p.setX(r.left());
break;
default: // top
p.setY(r.top());
break;
}
if (p.x() < 0)
p.setX(0);
if (p.x() > r.width())
p.setX(r.width());
if (p.y() < 0)
p.setY(0);
if (p.y() > r.height())
p.setY(r.height());
if (position.side() != newside) {
position.setSide(newside);
attributes.setValue("side", PinPosition::sideNames.at(newside));
updateOffset(snap(p).toPoint());
reorient();
// As the pin moves around a corner of a section, it can switch sides without its pos() changing.
// In that case the ItemPositionHasChanged event doesn't occur. So we need to emit the moved signal here.
emit moved();
}
}
}
}
return snap(p);
}
if (ItemPositionHasChanged == change && pageScene) {
updateOffset(pos().toPoint());
if (data(FreeSCH::SectionChanged).toBool()) {
setData(FreeSCH::SectionChanged, false);
emit sectionChanged(this, parentItem());
}
emit moved();
}
return QGraphicsItem::itemChange(change, value);
}
