00001
00027 #include <ObstacleAvoider.H>
00028 #include <RobotTypes.H>
00029
00030 using namespace std;
00031 using namespace WURDE;
00032
00033 void ObstacleAvoider::setAvoidanceRadius(double radius){
00034 Pose goalPose(0,0,0);
00035 requests.avoidanceRadius.setValue(radius);
00036 requests.goalPose.setValue(goalPose);
00037 requests.goalHasTheta.setValue(false);
00038 requests.goalHasLocation.setValue(false);
00039 requests.absolute.setValue(false);
00040
00041 publishRequest();
00042 }
00043
00044 void ObstacleAvoider::stop(){
00045 Pose goal_pose(0,0,0);
00046
00047 requests.goalPose.setValue(goal_pose);
00048 requests.goalHasTheta.setValue(true);
00049 requests.goalHasLocation.setValue(true);
00050 requests.absolute.setValue(false);
00051 requests.brake.setValue(true);
00052
00053 publishRequest();
00054 }
00055
00056 void ObstacleAvoider::moveToPoint(Point &goal){
00057 Pose goal_pose(goal.x(),goal.y());
00058
00059 requests.goalPose.setValue(goal_pose);
00060 requests.goalHasTheta.setValue(false);
00061 requests.goalHasLocation.setValue(true);
00062 requests.absolute.setValue(true);
00063 requests.brake.setValue(false);
00064
00065 publishRequest();
00066 }
00067
00068 void ObstacleAvoider::moveToPoint(const double &x, const double &y){
00069 Pose goal_pose(x,y);
00070
00071 requests.goalPose.setValue(goal_pose);
00072 requests.goalHasTheta.setValue(false);
00073 requests.goalHasLocation.setValue(true);
00074 requests.absolute.setValue(true);
00075 requests.brake.setValue(false);
00076
00077 publishRequest();
00078 }
00079
00080 void ObstacleAvoider::moveToPose(Pose &goal){
00081 requests.goalPose.setValue(goal);
00082 requests.goalHasTheta.setValue(true);
00083 requests.goalHasLocation.setValue(true);
00084 requests.absolute.setValue(true);
00085 requests.brake.setValue(false);
00086
00087 publishRequest();
00088 }
00089
00090 void ObstacleAvoider::moveToPose(const double &x, const double &y, const double &theta){
00091 Pose goal_pose(x,y,theta);
00092
00093 requests.goalPose.setValue(goal_pose);
00094 requests.goalHasTheta.setValue(true);
00095 requests.goalHasLocation.setValue(true);
00096 requests.absolute.setValue(true);
00097 requests.brake.setValue(false);
00098
00099 publishRequest();
00100 }
00101
00102 void ObstacleAvoider::rotateToAngle(const double &theta){
00103 Pose goal_pose(0,0,theta);
00104
00105 requests.goalPose.setValue(goal_pose);
00106 requests.goalHasTheta.setValue(true);
00107 requests.goalHasLocation.setValue(false);
00108 requests.absolute.setValue(true);
00109 requests.brake.setValue(false);
00110
00111 publishRequest();
00112 }
00113
00114 void ObstacleAvoider::moveToRelativePoint(Point &goal){
00115 Pose goal_pose(goal.x(),goal.y());
00116
00117 requests.goalPose.setValue(goal_pose);
00118 requests.goalHasTheta.setValue(false);
00119 requests.goalHasLocation.setValue(true);
00120 requests.absolute.setValue(false);
00121 requests.brake.setValue(false);
00122
00123 publishRequest();
00124 }
00125
00126 void ObstacleAvoider::moveToRelativePoint(const double &x, const double &y){
00127 Pose goal_pose(x,y);
00128
00129 requests.goalPose.setValue(goal_pose);
00130 requests.goalHasTheta.setValue(false);
00131 requests.goalHasLocation.setValue(true);
00132 requests.absolute.setValue(false);
00133 requests.brake.setValue(false);
00134
00135 publishRequest();
00136 }
00137
00138 void ObstacleAvoider::moveToRelativePose(Pose &goal){
00139 requests.goalPose.setValue(goal);
00140 requests.goalHasTheta.setValue(true);
00141 requests.goalHasLocation.setValue(true);
00142 requests.absolute.setValue(false);
00143 requests.brake.setValue(false);
00144
00145 publishRequest();
00146 }
00147
00148 void ObstacleAvoider::moveToRelativePose(const double &x, const double &y, const double &theta){
00149 Pose goal_pose(x,y,theta);
00150
00151 requests.goalPose.setValue(goal_pose);
00152 requests.goalHasTheta.setValue(true);
00153 requests.goalHasLocation.setValue(true);
00154 requests.absolute.setValue(false);
00155 requests.brake.setValue(false);
00156
00157 publishRequest();
00158 }
00159
00160 void ObstacleAvoider::rotateToRelativeAngle(const double &theta){
00161 Pose goal_pose(0,0,theta);
00162
00163 requests.goalPose.setValue(goal_pose);
00164 requests.goalHasTheta.setValue(true);
00165 requests.goalHasLocation.setValue(false);
00166 requests.absolute.setValue(false);
00167 requests.brake.setValue(false);
00168
00169 publishRequest();
00170 }
00171
00172
00173 void ObstacleAvoider::translateFixedDistance(const double &distance){
00174 Pose goal_pose(distance,0);
00175
00176 requests.goalPose.setValue(goal_pose);
00177 requests.goalHasTheta.setValue(false);
00178 requests.goalHasLocation.setValue(false);
00179 requests.absolute.setValue(false);
00180 requests.brake.setValue(false);
00181
00182 publishRequest();
00183 }
00184
00185 void ObstacleAvoider::rotateFixedAngle(const double &rotate){
00186 Pose goal_pose(0,0,rotate);
00187
00188 requests.goalPose.setValue(goal_pose);
00189 requests.goalHasTheta.setValue(true);
00190 requests.goalHasLocation.setValue(false);
00191 requests.absolute.setValue(false);
00192 requests.brake.setValue(false);
00193
00194 publishRequest();
00195 }
00196
