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)