Revision history [back]

Hey, I believe there is lil problem with logic, atan(y,x) gives you angle mapped from -3.14 to 3.14. So at any point of time your curr_head and heading value will be between -pi to pi. and n_head = curr_head-headding (lets take a cases) 1) both values from 0 to pi/2 -> diff will be > -3.14 so it will satisfy first condition and wont go for next (coz of else if) (this will behave same way for any angles with abs(diff) < pi ) It will move round and round in most cases .. and you should increase your threshold because the subscribed topic callback depends on the frequency you give. so if its not high enough then it will skip some poses. you can rearrange if conditions for your first problem, ( first try to write extreme conditions like >pi or <-pi)

NOTE : Its better if you map the data first to 0 -> 2*pi , and use abs(diff) to get diff in value and abs(diff)/diff to get direction. it would be lot easier with lot less equations.

Hope this helped Sudeep

Hey, I believe there is lil problem with logic, atan(y,x) gives you angle mapped from -3.14 to 3.14. So at any point of time your curr_head and heading value will be between -pi to pi. and n_head = curr_head-headding (lets take a cases) 1) both values from 0 to pi/2 -> diff will be > -3.14 so it will satisfy first condition and wont go for next (coz of else if) (this will behave same way for any angles with abs(diff) < pi ) It will move round and round in most cases .. and you should increase your threshold because the subscribed topic callback depends on the frequency you give. so if its not high enough then it will skip some poses. you can rearrange if conditions for your first problem, ( first try to write extreme conditions like >pi or <-pi)

NOTE : :Actually, n_head value ls between -2pi to 2pi, you should consider those cases too. Its better if you map the data first to 0 -> 2*pi , and use abs(diff) to get diff in value and abs(diff)/diff to get direction. it would be lot easier with lot less equations.

Hope this helped Sudeep