How to use initialpose orientation
i am still in the process of learning ros and i need some help. I have a task where i need to detect a obstacle(based on the obstacle color) from the pointcloud and move the turtlebot around the obstacle (also based on the color). The obstacles is a cylinder of a fixed radius (0.3m).
To acomplish this i have made one node which process the color and one which handles the navigation. When i detect a obstacle in the point cloud i convert the closest point into the map frame using a transform listener (this works). Then i use MoveBaseClient to send goals to the turtlebot.
The problem here is that i need to know where is the front and the back of the obstacle (so the turtlebot doesnt spin around the same obstacle). My solution to this problem is to set the initialPose (in rviz) which has the orientation perpendicular to the obstacle. This way i can compute 3 points: one in front of the obstacle, one on the left or on the right of the obstacle and one behind the obstacle. The problem is that this doesnt work as i planed and the points are not properly computed. The first problem is that the orientation i get is in form: x,y,z and only z is set. And also when i normalize the vector i lose the correct orientation (which is clearly seen below in the output). How can i construct a vector with the same direction as the orientation but of length of obstacle radius? The other thing is that the map itself has its own orientation.. do i have to consider that also? If someone could be so kind to help me understand what i am doing wrong. I have spent weeks on getting this to work.
The code for computing the points:
// vector of the obstacles center (coordinates are in map frame)
tf::Vector3 vec(ob->position.x, ob->position.y, 0);
// vector of the initialPose orientation
tf::Vector3 vec2(initialPose.orientation.x, initialPose.orientation.z, 0);
// normalize the orientation vector
vec2.normalize();
tf::Vector3 res;
// multiply the orientation vector with the obstacle radius
vec2 = vec2 * (ob->radius);
float angle,px,py; px = py = angle = 0;
switch (pointIndex) {
case 0: // point in front of the obstacle
// position + orientation
res = vec - vec2;
break;
case 1: // point on the left/right of the obstacle
if (ob->color == 2) {
angle = 1.57079633f; // 90 degrees
} else {
angle = -1.57079633f; // - 90 degrees
}
// rotate the vector
px = cos(angle) * vec2.getX() - sin(angle) * vec2.getY();
py = sin(angle) * vec2.getX() + cos(angle) * vec2.getY();
vec2.setX(px); vec2.setY(py);
// position + orientation
res = vec + vec2;
break;
case 2: // point behind the obstacle
// position + orientation
res = vec + vec2;
break;
}
// set the goal
setGoal(res);
Output of the code:
[ INFO] [1339592203.148984056]: new obstacle detected at X,Y,Z =
[-2.574487,2.275727,0.000000] with color 2
[ INFO] [1339592212.150162291]: Got orientation: 0.000000 0.000000 -0.376139 i ...
