I want to change turtlebot's velocity.
So i changed some values, but it doesn't works.
class GotoPoint():
def __init__(self):
rospy.init_node('turtlebot3_pointop_key', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
position = Point()
move_cmd = Twist()
r = rospy.Rate(10)
self.tf_listener = tf.TransformListener()
self.odom_frame = 'odom'
try:
self.tf_listener.waitForTransform(self.odom_frame, 'base_footprint', rospy.Time(), rospy.Duration(1.0))
self.base_frame = 'base_footprint'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
try:
self.tf_listener.waitForTransform(self.odom_frame, 'base_link', rospy.Time(), rospy.Duration(1.0))
self.base_frame = 'base_link'
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")
rospy.signal_shutdown("tf Exception")
(position, rotation) = self.get_odom()
last_rotation = 0
linear_speed = 1
angular_speed = 1
(goal_x, goal_y, goal_z) = self.getkey()
if goal_z > 180 or goal_z < -180:
print("you input wrong z range.")
self.shutdown()
goal_z = np.deg2rad(goal_z)
goal_distance = sqrt(pow(goal_x - position.x, 2) + pow(goal_y - position.y, 2))
distance = goal_distance
while distance > 0.05:
(position, rotation) = self.get_odom()
x_start = position.x
y_start = goal_y
path_angle = atan2(goal_y - y_start, goal_x- x_start)
if path_angle < -pi/4 or path_angle > pi/4:
if goal_y < 0 and y_start < goal_y:
path_angle = -2*pi + path_angle
elif goal_y >= 0 and y_start > goal_y:
path_angle = 2*pi + path_angle
if last_rotation > pi-0.1 and rotation <= 0:
rotation = 2*pi + rotation
elif last_rotation < -pi+0.1 and rotation > 0:
rotation = -2*pi + rotation
move_cmd.angular.z = angular_speed * path_angle-rotation
distance = sqrt(pow((goal_x - x_start), 2) + pow((goal_y - y_start), 2))
move_cmd.linear.x = min(linear_speed * distance, 0.1)
if move_cmd.angular.z > 0:
move_cmd.angular.z = min(move_cmd.angular.z, 1.5)
else:
move_cmd.angular.z = max(move_cmd.angular.z, -1.5)
last_rotation = rotation
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
goal_distance = sqrt(pow(goal_x - position.x, 2) + pow(goal_y - position.y, 2))
distance = goal_distance
while distance > 0.05:
(position, rotation) = self.get_odom()
x_start = goal_x
y_start = position.y
path_angle = atan2(goal_y - y_start, goal_x- x_start)
if path_angle < -pi/4 or path_angle > pi/4:
if goal_y < 0 and y_start < goal_y:
path_angle = -2*pi + path_angle
elif goal_y >= 0 and y_start > goal_y:
path_angle = 2*pi + path_angle
if last_rotation > pi-0.1 and rotation <= 0:
rotation = 2*pi + rotation
elif last_rotation < -pi+0.1 and rotation > 0:
rotation = -2*pi + rotation
move_cmd.angular.z = angular_speed * path_angle-rotation
distance = sqrt(pow((goal_x - x_start), 2) + pow((goal_y - y_start), 2))
move_cmd.linear.x = min(linear_speed * distance, 0.1)
if move_cmd.angular.z > 0:
move_cmd.angular.z = min(move_cmd.angular.z, 1.5)
else:
move_cmd.angular.z = max(move_cmd.angular.z, -1.5)
last_rotation = rotation
self.cmd_vel.publish(move_cmd)
r.sleep()
(position, rotation) = self.get_odom()
while abs(rotation - goal_z) > 0.05:
(position, rotation) = self.get_odom()
if goal_z >= 0:
if rotation <= goal_z and rotation >= goal_z - pi:
move_cmd.linear.x = 0.00
move_cmd.angular.z = 0.5
else:
move_cmd.linear.x = 0.00
move_cmd.angular.z = -0.5
else:
if rotation <= goal_z + pi and rotation > goal_z:
move_cmd.linear.x = 0.00
move_cmd.angular.z = -0.5
else:
move_cmd.linear.x = 0.00
move_cmd.angular.z = 0.5
self.cmd_vel.publish(move_cmd)
r.sleep()
rospy.loginfo("Stopping the robot...")
self.cmd_vel.publish(Twist())
def getkey(self):
x, y, z = input("| x | y | z |\n").split()
if x == 's':
self.shutdown()
x, y, z = [float(x), float(y), float(z)]
return x, y, z
def get_odom(self):
try:
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
rotation = euler_from_quaternion(rot)
except (tf.Exception, tf.ConnectivityException, tf.LookupException):
rospy.loginfo("TF Exception")
return
return (Point(*trans), rotation[2])
def shutdown(self):
self.cmd_vel.publish(Twist())
rospy.sleep(1)
I changed
move_cmd.linear.x = min(linear_speed * distance, 0.1)
into
move_cmd.linear.x = min(linear_speed * distance, 0.7)
but Instead of going to the target spot, the robot went round and round
I also changed linear_speed at 0.7, but the results was same.
How can i change the robot's speed?
Related
I got point cloud data in the form of [(x, y, z) , (norm_x, norm_y, norm_z)] in a text file. I am trying to convert this into a png or jpg image file where any points intensity corresponds to its depth (z).
here is how an stl 3d file looks like (left). On the right is what i am trying to make.
Thank you all for taking time to read this.
x_min = -2
x_max = 2
y_min = -1
y_max = 1
z_min = 0
z_max = 1
dx = x_max - x_min
dy = y_max - y_min
dz = z_max - z_min
Ps = []
for (i = 0; i < 1000; ++i) Ps.push([x_min + Math.random()*dx, y_min + Math.random()*dy, z_min + Math.random()*dz])
width = canvas.width
height = canvas.height
context = canvas.getContext('2d')
context.setFillColor('#000000')
context.fillRect(0, 0, width, height)
imagedata = context.getImageData(0, 0, width, height)
data = imagedata.data
w = width - 1
h = height - 1
for (P of Ps) {
col = Math.round(((P[0] - x_min)/dx)*w)
row = Math.round(((y_max - P[1])/dy)*h)
val = ((P[2] - z_min)/dz)*255
i = 4*(width*row + col)
if (data[i] < val) data[i] = data[i + 1] = data[i + 2] = val
}
context.putImageData(imagedata, 0, 0)
a.href = canvas.toDataURL()
<canvas id=canvas>HTML5</canvas><br><a id=a>Download</a>
I have a pair of Cartesian coordinates that represent a line in an image. I would like to convert this line to polar form and draw it over the image.
e.g
cv::Vec4f line {10,20,60,70};
float x1 = line[0];
float y1 = line[1];
float x2 = line[2];
float y2 = line[3];
I want this line to be represented in cv::Vec2f form(rho,theta).
Taking care of rho & theta with all possible slopes.
Given are the image dimensions :: w and h;
w = image.cols
h = image.rows
How can I achieve this.
N.B: We can also assume that the line can be an extended one running across the image.
for (size_t i = 0; i < lines.size(); i++)
{
int x1 = lines[i][0];
int y1 = lines[i][1];
int x2 = lines[i][2];
int y2 = lines[i][3];
float d = sqrt(((y1-y2)*(y1-y2)) + ((x2-x1)*(x2-x1)) );
float rho = (y1*x2 - y2*x1)/d;
float theta = atan2(x2 - x1,y1-y2) ;
if(rho < 0){
theta *= -1;
rho *= -1;
}
linv2f.push_back(cv::Vec2f(rho,theta));
}
The above approach doesnt give me results when I plot the lines I dont get the lines that are overlapping their original vec4f form.
I use this to convert vec2f to vec4f for testing :
cv::Vec4f cvtVec2fLine(const cv::Vec2f& data, const cv::Mat& img)
{
float const rho = data[0];
float const theta = data[1];
cv::Point pt1,pt2;
if((theta < CV_PI/4. || theta > 3. * CV_PI/4.)){
pt1 = cv::Point(rho / std::cos(theta), 0);
pt2 = cv::Point( (rho - img.rows * std::sin(theta))/std::cos(theta), img.rows);
}else {
pt1 = cv::Point(0, rho / std::sin(theta));
pt2 = cv::Point(img.cols, (rho - img.cols * std::cos(theta))/std::sin(theta));
}
cv::Vec4f l;
l[0] = pt1.x;
l[1] = pt1.y;
l[2] = pt2.x;
l[3] = pt2.y;
return l;
}
rho-theta equation has form
x * Cos(Theta) + y * Sin(Theta) - Rho = 0
We want to represent equation 'by two points' into rho-theta form (page 92 in pdf here). If we have
x * A + y * B - C = 0
and need coefficients in trigonometric form, we can divide all equation by magnitude of (A,B) coefficient vector.
D = Length(A,B) = Math.Hypot(A,B)
x * A/D + y * B/D - C/D = 0
note that (A/D)^2 + (B/D)^2 = 1 - basic trigonometric equality, so we can consider A/D and B/D as cosine and sine of some angle theta.
Your line equation is
(y-y1) * (x2-x1) - (x-x1) * (y2-y1) = 0
or
x * (y1-y2) + y * (x2-x1) - (y1 * x2 - y2 * x1) = 0
let
D = Sqrt((y1-y2)^2 + (x2-x1)^2)
so
Theta = ArcTan2(x2-x1, y1-y2)
Rho = (y1 * x2 - y2 * x1) / D
edited
If Rho is negative, change sign of Rho and shift Theta by Pi
Example:
x1=1,y1=0, x2=0,y2=1
Theta = atan2(-1,-1)=-3*Pi/4
D=Sqrt(2)
Rho=-Sqrt(2)/2 negative =>
Rho = Sqrt(2)/2
Theta = Pi/4
Back substitutuon - find points of intersection with axes
0 * Sqrt(2)/2 + y0 * Sqrt(2)/2 - Sqrt(2)/2 = 0
x=0 y=1
x0 * Sqrt(2)/2 + 0 * Sqrt(2)/2 - Sqrt(2)/2 = 0
x=1 y=0
I use quaternion from CMAttitude to rotate SceneKit camera.
Also I need to use Y rotation angle extracted from the quaternion.
I expected that it would be roll, but after extraction Y rotation angle corresponds to the pitch which has -90:90 range.
How can I convert this range to 0:180 or 0:360?
- (SCNQuaternion)SCNQuaternionFromCMQuaternion:(CMQuaternion)q {
GLKQuaternion Q = GLKQuaternionMake(q.x, q.y, q.z, q.w);
GLKQuaternion xRotQ = GLKQuaternionMakeWithAngleAndAxis(-M_PI_2, 1, 0, 0);
Q = GLKQuaternionMultiply(xRotQ, Q);
double roll = atan2(2.0 * (Q.y * Q.z - Q.w * Q.x), 1.0 - 2.0 * (Q.x * Q.x + Q.y * Q.y)); // 0:180 but around X
double pitch = RADIANS_TO_DEGREES(asin(-2.0f * (Q.x * Q.z + Q.w * Q.y))); // 0:90 around Y
NSLog(#"%f", pitch);
// ...
CMQuaternion rq = {.x = Q.x, .y = Q.y, .z = Q.z, .w = Q.w};
return SCNVector4Make(rq.x, rq.y, rq.z, rq.w);
}
I found this way:
- (SCNQuaternion)SCNQuaternionFromCMQuaternion:(CMQuaternion)q {
GLKQuaternion Q = GLKQuaternionMake(q.x, q.y, q.z, q.w);
GLKQuaternion xRotQ = GLKQuaternionMakeWithAngleAndAxis(-M_PI_2, 1, 0, 0);
Q = GLKQuaternionMultiply(xRotQ, Q);
double gx = 2.0 * (Q.y * Q.w - Q.x * Q.z);
//double gy = 2.0 * (Q.x * Q.y + Q.z * Q.w);
double gz = Q.x * Q.x - Q.y * Q.y - Q.z * Q.z + Q.w * Q.w;
double pitch = RADIANS_TO_DEGREES(-asin( -2.0 * (Q.y * Q.w - Q.x * Q.z)));
if (gx >= 0 && gz < 0)
pitch = 180 - pitch;
else if (gx < 0 && gz < 0)
pitch = 180 - pitch;
else if (gx < 0 && gz >= 0)
pitch = 360 + pitch;
NSLog(#"%f", pitch); // now it has 0-360 range
CMQuaternion rq = {.x = Q.x, .y = Q.y, .z = Q.z, .w = Q.w};
return SCNVector4Make(rq.x, rq.y, rq.z, rq.w);
}
I'm trying to implement fast voxel traversal algorithm and calculate T and M according to this answer (T is tDelta, M is tMax). All is good if the two components of the direction vector V are positive. But if at least one of them is negative, it's work wrong.
Green point is start, red is end. All seems correct.
And now from bigger to less position.
Traversal method:
private IEnumerable<Vector2> GetCrossedCells(Vector2 pPoint1, Vector2 pPoint2)
{
Vector2 V = pPoint2 - pPoint1; // direction & distance vector
if (V != Vector2.Zero)
{
Vector2 U = Vector2.Normalize(V); // direction unit vector
Vector2 S = new Vector2(Math.Sign(U.X), Math.Sign(U.Y)); // sign vector
Vector2 P = pPoint1; // position
Vector2 G = new Vector2((int) Math.Floor(P.X / CELL_SIZE), (int) Math.Floor(P.Y / CELL_SIZE)); // grid coord
Vector2 T = new Vector2(Math.Abs(CELL_SIZE / U.X), Math.Abs(CELL_SIZE / U.Y));
Vector2 M = new Vector2(
Single.IsInfinity(T.X) ? Single.PositiveInfinity : T.X * (1.0f - (P.X / CELL_SIZE) % 1),
Single.IsInfinity(T.Y) ? Single.PositiveInfinity : T.Y * (1.0f - (P.Y / CELL_SIZE) % 1));
Vector2 D = Vector2.Zero;
bool isCanMoveByX = S.X != 0;
bool isCanMoveByY = S.Y != 0;
while (isCanMoveByX || isCanMoveByY)
{
yield return G;
D = new Vector2(
S.X > 0 ? (float) (Math.Floor(P.X / CELL_SIZE) + 1) * CELL_SIZE - P.X :
S.X < 0 ? (float) (Math.Ceiling(P.X / CELL_SIZE) - 1) * CELL_SIZE - P.X :
0,
S.Y > 0 ? (float) (Math.Floor(P.Y / CELL_SIZE) + 1) * CELL_SIZE - P.Y :
S.Y < 0 ? (float) (Math.Ceiling(P.Y / CELL_SIZE) - 1) * CELL_SIZE - P.Y :
0);
if (Math.Abs(V.X) <= Math.Abs(D.X))
{
D.X = V.X;
isCanMoveByX = false;
}
if (Math.Abs(V.Y) <= Math.Abs(D.Y))
{
D.Y = V.Y;
isCanMoveByY = false;
}
if (M.X <= M.Y)
{
M.X += T.X;
G.X += S.X;
if (isCanMoveByY)
{
D.Y = U.Y / U.X * D.X; // U.X / U.Y = D.X / D.Y => U.X * D.Y = U.Y * D.X
}
}
else
{
M.Y += T.Y;
G.Y += S.Y;
if (isCanMoveByX)
{
D.X = U.X / U.Y * D.Y;
}
}
V -= D;
P += D;
}
}
}
In debug I can see that for example M.Y > M.X when should be the opposite if S.X < 0 or S.Y < 0.
Tell me please what my code work wrong for negative directions?
So, I solve it.
I make code cleaner and problem is gone.
private IEnumerable<Vector2> GetCrossedCells(Vector2 pPoint1, Vector2 pPoint2)
{
if (pPoint1 != pPoint2)
{
Vector2 V = (pPoint2 - pPoint1) / CELL_SIZE; // direction & distance vector
Vector2 U = Vector2.Normalize(V); // direction unit vector
Vector2 S = new Vector2(Math.Sign(U.X), Math.Sign(U.Y)); // sign vector
Vector2 P = pPoint1 / CELL_SIZE; // position in grid coord system
Vector2 G = new Vector2((int) Math.Floor(P.X), (int) Math.Floor(P.Y)); // grid coord
Vector2 T = new Vector2(Math.Abs(CELL_SIZE / U.X), Math.Abs(CELL_SIZE / U.Y));
Vector2 D = new Vector2(
S.X > 0 ? 1 - P.X % 1 : S.X < 0 ? P.X % 1 : 0,
S.Y > 0 ? 1 - P.Y % 1 : S.Y < 0 ? P.Y % 1 : 0);
Vector2 M = new Vector2(
Single.IsInfinity(T.X) || S.X == 0 ? Single.PositiveInfinity : T.X * D.X,
Single.IsInfinity(T.Y) || S.Y == 0 ? Single.PositiveInfinity : T.Y * D.Y);
bool isCanMoveByX = S.X != 0;
bool isCanMoveByY = S.Y != 0;
while (isCanMoveByX || isCanMoveByY)
{
yield return G;
D = new Vector2(
S.X > 0 ? (float) Math.Floor(P.X) + 1 - P.X :
S.X < 0 ? (float) Math.Ceiling(P.X) - 1 - P.X :
0,
S.Y > 0 ? (float) Math.Floor(P.Y) + 1 - P.Y :
S.Y < 0 ? (float) Math.Ceiling(P.Y) - 1 - P.Y :
0);
if (Math.Abs(V.X) <= Math.Abs(D.X))
{
D.X = V.X;
isCanMoveByX = false;
}
if (Math.Abs(V.Y) <= Math.Abs(D.Y))
{
D.Y = V.Y;
isCanMoveByY = false;
}
if (M.X <= M.Y)
{
M.X += T.X;
G.X += S.X;
if (isCanMoveByY)
{
D.Y = U.Y / U.X * D.X; // U.X / U.Y = D.X / D.Y => U.X * D.Y = U.Y * D.X
}
}
else
{
M.Y += T.Y;
G.Y += S.Y;
if (isCanMoveByX)
{
D.X = U.X / U.Y * D.Y;
}
}
V -= D;
P += D;
}
}
}
Update
I'm began from removing redundant divisions on GRID_CELL and then notice mistake in M calculation.
There are using Frac() function in answer to the question, a link to which I provided. I'm calculate it like (1 - P % 1), but that is a case for S > 0, and there are should be (P % 1) if S < 0, and Inf for S = 0.
Update 2
Also there should be
Vector2 D = new Vector2(
S.X > 0 ? (float) Math.Floor(P.X) + 1 - P.X :
S.X < 0 ? (float) Math.Ceiling(P.X) - 1 - P.X :
0,
S.Y > 0 ? (float) Math.Floor(P.Y) + 1 - P.Y :
S.Y < 0 ? (float) Math.Ceiling(P.Y) - 1 - P.Y :
0);
Instead of
Vector2 D = new Vector2(
S.X > 0 ? 1 - P.X % 1 : S.X < 0 ? P.X % 1 : 0,
S.Y > 0 ? 1 - P.Y % 1 : S.Y < 0 ? P.Y % 1 : 0);
Because M will be infinity in case S < 0 and P haven't fractional part.
I'm trying to obtain the orientation of a shape (binary or contour). The shape is mostly rectangular but has a large hole on one side. I want my orientation to be consistent with this assymetry in the object.
I've been looking at several articles that use the spatial and central moments for this. E.g.
Binary Image Orientation
but it seems that the orientation I get with this is sometimes off with a multiple of 90 degrees.
The following document states that there is some ambiguity:
http://public.cranfield.ac.uk/c5354/teaching/dip/opencv/SimpleImageAnalysisbyMoments.pdf
If I implement this using
private void GetCenterAndOrientationViaMoments(Contour<Point> cont, Size imageSize)
{
// obtain the orientation of the found object
// first draw the binary blob in a separate image
// I do this for the hole(s) in the image, but I'm not sure if it is needed.
// Possibly I can tak the moments directly from the contour
Image<Gray, byte> instanceImage = new Image<Gray, byte>(imageSize);
instanceImage.FillConvexPoly(cont.ToArray(), new Gray(255));
for (Contour<Point> hole = cont.VNext;
hole != null;
hole = hole.HNext)
instanceImage.FillConvexPoly(hole.ToArray(), new Gray(0));
// calculate the moments
MCvMoments m = instanceImage.GetMoments(true);
// MCvMoments m = cont.GetMoments();
double m00 = m.GetSpatialMoment(0, 0);
double m10 = m.GetSpatialMoment(1, 0);
double m01 = m.GetSpatialMoment(0, 1);
double mu11 = m.GetCentralMoment(1, 1);
double mu20 = m.GetCentralMoment(2, 0);
double mu02 = m.GetCentralMoment(0, 2);
// calculate the center
PointF center = new PointF((float)(m10 / m00), (float)(m01 / m00));
// calculate the orientation
// http://public.cranfield.ac.uk/c5354/teaching/dip/opencv/SimpleImageAnalysisbyMoments.pdf
double theta = 0;
double mu20_mu02 = (mu20 - mu02);
if ((mu20_mu02 == 0) & (mu11 == 0))
theta = 0;
else if ((mu20_mu02 == 0) & (mu11 > 0))
theta = Math.PI / 4;
else if ((mu20_mu02 == 0) & (mu11 < 0))
theta = -Math.PI / 4;
else if ((mu20_mu02 > 0) & (mu11 == 0))
theta = 0;
else if ((mu20_mu02 < 0) & (mu11 == 0))
theta = -Math.PI / 2;
else if ((mu20_mu02 > 0) & (mu11 > 0))
theta = 0.5 * Math.Atan((2 * mu11) / mu20_mu02);
else if ((mu20_mu02 > 0) & (mu11 < 0))
theta = 0.5 * Math.Atan((2 * mu11) / mu20_mu02);
else if ((mu20_mu02 < 0) & (mu11 > 0))
theta = 0.5 * Math.Atan((2 * mu11) / mu20_mu02) + Math.PI / 2;
else if ((mu20_mu02 < 0) & (mu11 < 0))
theta = 0.5 * Math.Atan((2 * mu11) / mu20_mu02) - Math.PI / 2;
#if DEBUG
int radius = 25;
instanceImage.Draw(new CircleF(center, radius), new Gray(100), 2);
instanceImage.Draw(
new LineSegment2DF(
center,
new PointF(
(float)(center.X + radius * Math.Cos(theta)),
(float)(center.Y + radius * Math.Sin(theta)))),
new Gray(100),
2);
ImageViewer.Show(instanceImage, string.Format("Center and orientation"));
#endif
}
My orientation is correct, but does not always point to the same end of the object. In other words, I'm sometimes of by 180 degrees.
I'm guessing the method cannot provide exactly what I want because it uses the covariance of the distribution (http://en.wikipedia.org/wiki/Image_moments#Examples_2) which gives does not take into account the assymmetry caused by the hole, am I right?
Is there a way to resolve the 180 degrees ambiguity?
Regards,
Tom