class Controller:
k = 0.5
k2 = 2
def __init__(self, robot_name, goal):
file_name = "data_{}.txt".format(robot_name)
self.file = open(file_name, 'w')
sub_topic = "/{}/north_star".format(robot_name)
pub_topic = "/{}/cmd_vel".format(robot_name)
srv_topic = "/{}/gotogoal".format(robot_name)
self.sub_pose = rospy.Subscriber(sub_topic, NorthStarReadings, self.point_callback)
self.pub_vels = rospy.Publisher(pub_topic, Twist, queue_size=10)
self.go_to_goal = rospy.Service(srv_topic, GoToGoal, self.go_to_goal_handler)
self.goal = goal
self.clock = Clock()
self.work()
def __del__(self):
self.file.close()
def go_to_goal_handler(self, req):
self.goal.x = req.point.x
self.goal.y = req.point.y
print("New goal is [{}, {}]".format(self.goal.x, self.goal.y))
def point_callback(self, cur_pose):
t, dt = self.clock.getTandDT()
q = cur_pose.pose.orientation
eu = euler_from_quaternion((q.x, q.y, q.z, q.w))
lock.acquire()
e_x = self.goal.x - cur_pose.pose.position.x
e_y = self.goal.y - cur_pose.pose.position.y
e_theta = 0 - eu[2] # self.goal.z - eu[2]
lock.release()
e = sqrt(e_x ** 2 + e_y ** 2) # euclid error
# self.file.write("{} {} {} {}\n".format(t, cur_pose.pose.position.x, cur_pose.pose.position.y, e))
velocity = Twist()
if abs(e) > 0.05:
# velocity.linear.x = self.k * e_y
# velocity.linear.y = -self.k * e_x
velocity.linear.x = self.k * e_x
velocity.linear.y = self.k * e_y
velocity.angular.z = 4 * e_theta
self.pub_vels.publish(velocity)
def work(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
rate.sleep()
class Controller:
def __init__(self, robot_name, v, R, center=None):
file_name = "data_{}.txt".format(robot_name)
self.file = open(file_name, 'w')
sub_topic = "/{}/north_star".format(robot_name)
pub_topic = "/{}/cmd_vel".format(robot_name)
self.sub_pose = rospy.Subscriber(sub_topic, NorthStarReadings,
self.control_callback)
self.pub_vels = rospy.Publisher(pub_topic, Twist, queue_size=10)
# Trajectory setup.
self.circle_law = CircleControlLaw(v, R)
if center:
self.center = center
else:
self.center = (0, 0)
self.clock = Clock()
self.state = (0, 0, 0)
# self.work()
def __del__(self):
self.file.close()
def set_center(self, center):
self.center = center
def control_callback(self, cur_pose):
t, dt = self.clock.getTandDT()
lock.acquire()
p = cur_pose.pose.position
q = cur_pose.pose.orientation
cur_theta = euler_from_quaternion((q.x, q.y, q.z, q.w))[2]
ux, uy, e = self.circle_law.getControl(
cur_pose.pose.position.x, cur_pose.pose.position.y, cur_theta,
(self.center[0], self.center[1]))
self.state = (p.x, p.y, cur_theta)
lock.release()
velocity = Twist()
velocity.linear.x = ux
velocity.linear.y = uy
self.pub_vels.publish(velocity)
self.file.write("{} {} {} {}\n".format(t, cur_pose.pose.position.x,
cur_pose.pose.position.y, e))
def work(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
rate.sleep()
class Controller:
def __init__(self, robot_name, v, beta, phi):
"""
var state = (xcur, ycur, thetacur)
:param robot_name:
:param v:
:param beta:
:param phi:
"""
file_name = "data_{}.txt".format(robot_name)
self.file = open(file_name, 'w')
sub_topic = "/{}/north_star".format(robot_name)
pub_topic = "/{}/cmd_vel".format(robot_name)
self.sub_pose = rospy.Subscriber(sub_topic, NorthStarReadings,
self.control_callback)
self.pub_vels = rospy.Publisher(pub_topic, Twist, queue_size=10)
# Trajectory setup.
self.line_law = LineControlLaw(v, beta, phi)
self.clock = Clock()
self.state = (0, 0, 0)
self.turn = "ON"
self.old_cur_pose = None
# self.work()
def __del__(self):
self.file.close()
def control_callback(self, cur_pose):
# if self.old_cur_pose:
# cur_pose.pose.position.x = 0.6 * cur_pose.pose.position.x + 0.4 * self.old_cur_pose.pose.position.x
# cur_pose.pose.position.y = 0.6 * cur_pose.pose.position.y + 0.4 * self.old_cur_pose.pose.position.y
# self.old_cur_pose = cur_pose
t, dt = self.clock.getTandDT()
lock.acquire()
p = cur_pose.pose.position
q = cur_pose.pose.orientation
cur_theta = euler_from_quaternion((q.x, q.y, q.z, q.w))[2]
ux, uy, e = self.line_law.getControl(cur_pose.pose.position.x,
cur_pose.pose.position.y,
cur_theta)
self.state = (p.x, p.y, cur_theta)
lock.release()
if self.turn == "ON":
velocity = Twist()
velocity.linear.x = ux
velocity.linear.y = uy
self.pub_vels.publish(velocity)
self.file.write("{} {} {} {}\n".format(t, cur_pose.pose.position.x,
cur_pose.pose.position.y,
e))
def work(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
rate.sleep()
if __name__ == "__main__":
"""
Work of this script are
'robot1' following given trajectory,
'robot2' moving around 'robot1' for circle trajectory.
"""
rospy.init_node('composite_node')
clock = Clock()
# if len(sys.argv) > 3:
# robot_name = sys.argv[1]
# (x0, y0) = sys.argv[2:]
some_curve = LineCntrl('robot1', 0.1, -1.57, 0)
circle_controller = CircleCntrl('robot2',
0.8,
0.5,
center=(float(0), float(0)))
rate = rospy.Rate(100)
while not rospy.is_shutdown():
t, dt = clock.getTandDT()
print(dt)
lock.acquire()
(x_r1, y_r1, __) = some_curve.state
circle_controller.set_center((x_r1, y_r1))
lock.release()
rate.sleep()
# else:
# print("Usage: composite_node.py x y \n (x,y) coordinates of 'robot1'.")
