def joy_callback(self, joy_msg):
# simple check for invalid joy_msg
if len(joy_msg.axes) < 2:
rospy.logerror("joy_msg axes array length (%d) has less than expected length (2)", len(joy_msg.axes))
return
# convert the joystick message to a velocity
cmd_vel_msg = Velocity()
cmd_vel_msg.linear = joy_msg.axes[1]*self.linear_scale
cmd_vel_msg.angular = joy_msg.axes[0]*self.angular_scale
# publish the velocity command message
self.cmd_vel_pub.publish(cmd_vel_msg)
def joy_callback(self, joy_msg):
# simple check for invalid joy_msg
if len(joy_msg.axes) < 2:
rospy.logerror(
"joy_msg axes array length (%d) has less than expected length (2)",
len(joy_msg.axes))
return
# convert the joystick message to a velocity
cmd_vel_msg = Velocity()
cmd_vel_msg.linear = joy_msg.axes[1] * self.linear_scale
cmd_vel_msg.angular = joy_msg.axes[0] * self.angular_scale
# publish the velocity command message
self.cmd_vel_pub.publish(cmd_vel_msg)
def talker():
pub = rospy.Publisher('turtle1/command_velocity', Velocity)
rospy.init_node('turtle_stdin')
while not rospy.is_shutdown():
str = getLine()
#rospy.loginfo(str)
pub.publish(Velocity(str[0], str[1]))
def turtle2_orbiting():
""" Make turtle2 do a square in the environment, reverse direction as turtle1 """
yield WaitCondition(lambda: sched.turtle2_pose is not None)
coverage_tid = sched.new_task(turtle1_coverage())
orbit_tid = sched.new_task(turtle2_orbit((5, 5.5)))
while True:
yield WaitCondition(
lambda: dist(sched.turtle1_pose, sched.turtle2_pose) < 1)
sched.printd('Met friend, exchange data')
sched.pause_task(orbit_tid)
turtle2_velocity.publish(Velocity(0, 0))
sched.kill_task(coverage_tid)
if sched.turtle1_pose.y < 5.5:
align_tid = sched.new_task(turtle1_align(math.pi / 2))
yield WaitCondition(lambda: abs(
angle_diff(sched.turtle1_pose.theta, math.pi / 2)) < 0.1)
sched.kill_task(align_tid)
target = (sched.turtle1_pose.x, sched.turtle1_pose.y + 3)
else:
align_tid = sched.new_task(turtle1_align(-math.pi / 2))
yield WaitCondition(lambda: abs(
angle_diff(sched.turtle1_pose.theta, -math.pi / 2)) < 0.1)
sched.kill_task(align_tid)
target = (sched.turtle1_pose.x, sched.turtle1_pose.y - 3)
go_tid = sched.new_task(turtle1_go(target))
yield WaitCondition(lambda: dist(sched.turtle1_pose, target) < 0.1)
sched.kill_task(go_tid)
sched.printd('Meeting done')
sched.resume_task(orbit_tid)
coverage_tid = sched.new_task(turtle1_coverage())
yield WaitDuration(6)
def joy_callback(self, joy_msg):
if len(joy_msg.axes) < 2:
rospy.logerror("bad array size")
return
if joy_msg.buttons[1] & joy_msg.buttons[6]:
# Trigger time reset, profile will begin on button up
rospy.loginfo("Starting Lissajous:")
self.t_start = 0
self.profile = 3
if joy_msg.buttons[1] & joy_msg.buttons[5]:
# Trigger time reset, profile will begin on button up
rospy.loginfo("Making Waves:")
self.t_start = 0
self.profile = 2
if joy_msg.buttons[1] & joy_msg.buttons[4]:
# Trigger time reset, profile will begin on button up
rospy.loginfo("Making an L:")
self.t_start = 0
self.profile = 1
if joy_msg.buttons[1]:
self.run_ok = self.profile
else:
self.run_ok = 0
self.t_start = 999.0
# enable joystick control if push button zero instead
if joy_msg.buttons[0] & ~joy_msg.buttons[1]:
if sim:
cmd_vel_msg = Velocity()
cmd_vel_msg.linear = joy_msg.axes[1]*self.linear_scale
cmd_vel_msg.angular = joy_msg.axes[0]*self.angular_scale
else:
cmd_vel_msg = Twist()
cmd_vel_msg.linear.x = joy_msg.axes[1]*self.linear_scale
cmd_vel_msg.angular.z = joy_msg.axes[0]*self.angular_scale
self.cmd_vel_pub.publish(cmd_vel_msg)
def pose_callback(data):
# centre on 5,7
dx = data.x - 5.0
dy = data.y - 7.0
# calculate radius
radius = sqrt(dx*dx + dy*dy)
# PID control for constant radius
u = steering_pid.update(radius, 2.5, rospy.get_rostime().to_sec())
# saturate
u = rospidlib.saturate(u,0.3)
# command
v = Velocity()
v.linear = 0.3
v.angular = -u
# send it
vel_pub.publish(v)
# tell the world
rospy.loginfo('Got pos = (%f,%f) rad=%f ctrl=%f', dx, dy, radius, u)
def turtle1_align(target_angle):
""" Make turtle1 align to angle, giving new speed command every second """
diff = angle_diff(sched.turtle1_pose.theta, target_angle)
sign_diff = math.copysign(1, diff)
while True:
# set new speed commands
turtle1_velocity.publish(Velocity(0, -sign_diff))
# wait for 1 s
yield WaitDuration(1)
def control_command(pose, dest_pos, speed=1.0):
''' Compute velocity given current pose and target '''
p = np.array((pose.x, pose.y))
o = np.array((math.cos(pose.theta), math.sin(pose.theta)))
t_g = np.array(dest_pos)
d_t = unit(t_g - p)
v_forward = np.dot(d_t, o)
v_rot = np.dot(d_t, perp(o))
return Velocity(v_forward * speed, v_rot * speed * 2)
def cupidon():
""" When turtles are close, make them dance """
my_tid = sched.get_current_tid()
while True:
yield WaitCondition(
lambda: dist(sched.turtle1_pose, sched.turtle2_pose) < 1)
sched.printd('Found friend, let\'s dance')
paused_tasks = sched.pause_all_tasks_except([my_tid])
turtle1_set_pen(255, 0, 0, 0, 0)
turtle2_set_pen(0, 255, 0, 0, 0)
for i in range(7):
turtle1_velocity.publish(Velocity(1, 1))
turtle2_velocity.publish(Velocity(1, -1))
yield WaitDuration(0.9)
turtle1_set_pen(0, 0, 0, 0, 1)
turtle2_set_pen(0, 0, 0, 0, 1)
sched.printd('Tired of dancing, going back to wandering')
sched.resume_tasks(paused_tasks)
yield WaitDuration(10)
def start():
global turtle_vel, vel, turtle_teleport_absolute, turtle_teleport_relative, turtle_setpen, pen, turtle_pen
vel = Velocity()
pen = Pen()
rospy.init_node('turtle_controller', anonymous=True)
rospy.wait_for_service('teleport_absolute')
turtle_teleport_absolute = rospy.ServiceProxy('teleport_absolute', TeleportAbsolute)
rospy.wait_for_service('teleport_relative')
turtle_teleport_relative = rospy.ServiceProxy('teleport_relative', TeleportRelative)
rospy.wait_for_service('set_pen')
turtle_setpen = rospy.ServiceProxy('set_pen', SetPen)
pen.r = 255
pen.g = 255
pen.b = 255
pen.width = 2
pen.off = 0
turtle_vel = rospy.Publisher('command_velocity', Velocity)
turtle_pen = rospy.Publisher('pen', Pen)
sleep(0.7)
setpen()
sleep(0.3)
def run_shape(self):
while not rospy.is_shutdown():
sleep_time = 0.1
lin_cmd = 0.0
ang_cmd = 0.0
self.t_start = self.t_start + sleep_time # assumes deteministic task execution
if self.run_ok == 1:
if self.t_start < 3.0:
lin_cmd = self.linear_scale
elif self.t_start < 4.0:
ang_cmd = self.angular_scale
elif self.t_start < 6.0:
lin_cmd = self.linear_scale
elif self.run_ok == 2:
if self.t_start < 5.0:
val = self.t_start
lin_cmd = self.linear_scale
ang_cmd = 0.9*math.sin(val*2)*self.angular_scale
elif self.run_ok == 3:
#rospy.loginfo("Running Shape Time: %f", self.t_start)
time_scale = 4.5 # rate to execute (cannot exceed 0.5m/sec wheel speed)
lissa_dur = math.pi*2*time_scale
xbox = 4.0/2;
ybox = 5.0/2;
corner = 1.0;
if self.t_start < lissa_dur:
a = 3.0/time_scale
b = 4.0/time_scale
mag_a = .4
mag_b = .3
offset = (math.pi/2/a) # start near corner
lastx = mag_a*math.sin( (self.t_start+offset-sleep_time)*a )
xnow = mag_a*math.sin( (self.t_start+offset)*a )
nextx = mag_a*math.sin( (self.t_start+offset+sleep_time)*a )
lastdx = xnow-lastx;
desdx = nextx-xnow;
#rospy.loginfo("%f %f %f :: %f %f",lastx,xnow,nextx,lastdx,desdx)
lasty = mag_b*math.sin( (self.t_start+offset-sleep_time)*b )
ynow = mag_b*math.sin( (self.t_start+offset)*b )
nexty = mag_b*math.sin( (self.t_start+offset+sleep_time)*b )
lastdy = ynow-lasty;
desdy = nexty-ynow;
#rospy.loginfo("%f %f %f :: %f %f",lasty,ynow,nexty,lastdy,desdy)
ang_cmd = math.atan2(desdy,desdx) - math.atan2(lastdy,lastdx)
orig_ang = ang_cmd
if abs(ang_cmd - self.last_ang_cmd) > math.pi:
ang_cmd = ang_cmd-2*math.pi*self.signum(ang_cmd-self.last_ang_cmd)
#rospy.loginfo("Unwrap: %f to %f",orig_ang*180/math.pi,ang_cmd*180/math.pi)
self.last_ang_cmd = ang_cmd
ang_cmd = ang_cmd / sleep_time
lin_cmd = math.sqrt(desdy*desdy + desdx*desdx) / sleep_time
#rospy.loginfo("%f %f",orig_ang*180/math.pi,ang_cmd*180/math.pi)
#rospy.loginfo("Command: lin %f, ang %f at %f",lin_cmd,ang_cmd,self.t_start)
elif self.t_start < lissa_dur+xbox:
lin_cmd = 2*self.linear_scale
elif self.t_start < lissa_dur+corner+xbox:
ang_cmd = -self.angular_scale
elif self.t_start < lissa_dur+ybox+corner+xbox:
lin_cmd = 2*self.linear_scale
elif self.t_start < lissa_dur+corner*2+ybox+xbox:
ang_cmd = -self.angular_scale
elif self.t_start < lissa_dur+xbox+2*corner+ybox+xbox:
lin_cmd = 2*self.linear_scale
elif self.t_start < lissa_dur+corner+xbox+2*corner+ybox+xbox:
ang_cmd = -self.angular_scale
elif self.t_start < lissa_dur+ybox+corner+xbox+2*corner+ybox+xbox:
lin_cmd = 2*self.linear_scale
elif self.t_start < lissa_dur+corner*2+ybox+xbox+2*corner+ybox+xbox:
ang_cmd = -self.angular_scale
if self.run_ok > 0:
if sim:
cmd_vel_msg = Velocity()
cmd_vel_msg.linear = lin_cmd
cmd_vel_msg.angular = ang_cmd
else:
cmd_vel_msg = Twist()
cmd_vel_msg.linear.x = lin_cmd
cmd_vel_msg.angular.z = ang_cmd
#math.cos(x)
#math.pi
#math.fabs(x)
# Keep this in run_ok to allow joy callback to be a joystick
self.cmd_vel_pub.publish(cmd_vel_msg)
rospy.sleep(sleep_time)
def orbit_command(pose, center, speed=1.0, d=2.5):
''' Give a command such as orbiting around center '''
c_d = dist(pose, center)
d_d = c_d - d
turn = max(0, d_d)
return Velocity(speed, -turn * turn * 0.1)
