# rospy.loginfo('smm: dx '+str(x_new-self.x))
y_new = (self.y - v_hat/w_hat*math.cos(self.theta)
- v_hat/w_hat*math.cos(self.theta+w_hat*dt))
# rospy.loginfo('smm: dy '+str(y_new-self.y))
theta_new = self.theta + w_hat*dt + y_hat*dt
new_odom = Odometry()
new_odom.pose.pose.position.x = x_new
new_odom.pose.pose.position.y = y_new
new_odom.pose.pose.orientation = heading_to_quaternion(theta_new)
new_odom.twist.twist.linear.x = v_hat
new_odom.twist.twist.angular.z = w_hat
new_odom.header.frame_id = self.frame_id
return new_odom
# pylint: disable=unused-argument
# it will be used when implmenented properly
def sample_normal(self, unused_b):
# TODO(buckbaskin): change to sample normal distribution
# norm(mean=0, stdev = b)
return 0
if __name__ == '__main__':
# pylint: disable=invalid-name
# leader is a fine name, it's not a constant
leader = ForceLeader()
leader.run_server(20)
des_speed = .5 # m/s
dx = end.pose.pose.position.x - start.pose.pose.position.x
dy = end.pose.pose.position.y - start.pose.pose.position.y
heading = math.atan2(dy, dx)
dx = des_speed*math.cos(heading)*dt
dy = des_speed*math.sin(heading)*dt
distance = math.sqrt(dx*dx+dy*dy)
steps = math.floor(distance/des_speed)
for i in range(1, int(steps)):
odo = Odometry()
odo.header.frame_id = 'odom'
odo.pose.pose.point = Point(x=start.x+i*dx, y=start.y+i*dy)
odo.pose.pose.orientation = heading_to_quaternion(heading)
odo.twist.twist.linear = Vector3(x=des_speed)
odo.twist.twist.angular = Vector3()
self.targets.append(odo)
if rvs:
self.index = len(self.targets)-2
else:
self.index = 0
if __name__ == '__main__':
# pylint: disable=invalid-name
# leader is a fine name, it's not a constant
leader = ExampleLeader()
leader.run_server()