def cmd_callback(cmd): """ Handle velocity commands from the navigation controllers """ left_motor = cmd.linear.x - cmd.angular.z right_motor = cmd.linear.x + cmd.angular.z motors_cmd = motors(left=left_motor, right=right_motor) motors_pub.publish(motors_cmd)
def manual_control_callback(data): #http://wiki.ros.org/joy#Microsoft_Xbox_360_Wireless_Controller_for_Linux axes = data.axes drivetrain_msg = motors() # RT if (1 - axes[5]) < 0.1: throttle = 0 else: throttle = (1 - axes[5]) * max_speed # LT if (1 - axes[2]) > 0.1: throttle = -(1 - axes[2]) * max_speed turning = axes[0] * max_speed drivetrain_msg.left = clamp(throttle - turning * 0.6, -max_speed, max_speed) drivetrain_msg.right = clamp(throttle + turning * 0.6, -max_speed, max_speed) # Corrections drivetrain_msg.right = -drivetrain_msg.right # rospy.loginfo("manual_control callback.") manual_control_pub.publish(drivetrain_msg)
def manual_control_callback(data): #http://wiki.ros.org/joy#Microsoft_Xbox_360_Wireless_Controller_for_Linux axes = data.axes drivetrain_msg = motors() drivetrain_msg.right = axes[4]**3 * 2.2 if abs(axes[4]) < 0.1: drivetrain_msg.right = 0 drivetrain_msg.left = axes[1]**3 * 2.2 if abs(axes[1]) < 0.1: drivetrain_msg.left = 0 # rospy.loginfo("manual_control callback.") manual_control_pub.publish(drivetrain_msg)
def timer_callback(event): if pos is None or heading is None: return cur_pos = (pos[0], pos[1]) lookahead = None radius = 0.3 # Start with a radius of 0.3 meters while lookahead is None and radius <= 3: # Look until we hit 3 meters max lookahead = pp.get_lookahead_point(cur_pos[0], cur_pos[1], radius) radius *= 1.2 if SHOW_PLOTS: draw_pp(cur_pos, lookahead, pp.path) if lookahead is not None: # Get heading to to lookahead from current position heading_to_lookahead = math.degrees(math.atan2(lookahead[1] - cur_pos[1], lookahead[0] - cur_pos[0])) if heading_to_lookahead < 0: heading_to_lookahead += 360 # Get difference in our heading vs heading to lookahead heading_error = heading_to_lookahead - heading heading_error = (heading_error + 180) % 360 - 180 # Normalize error to -1 to 1 scale error = heading_error/180 # Base forward velocity for both wheels forward_speed = 0.8 * (1 - abs(error))**5 # Define wheel linear velocities # Add proprtional error for turning. # TODO: PID instead of just P motor_pkt = motors() motor_pkt.left = (forward_speed - 0.4 * error) / WHEEL_RADIUS motor_pkt.right = (forward_speed + 0.4 * error) / WHEEL_RADIUS publy.publish(motor_pkt)
def timer_callback(event): if pos is None or heading is None: return cur_pos = (pos[0], pos[1]) lookahead = None radius = 0.8 # Starting radius while lookahead is None and radius <= 3: # Look until we hit 3 meters max lookahead = pp.get_lookahead_point(cur_pos[0], cur_pos[1], radius) radius *= 1.2 if SHOW_PLOTS: draw_pp(cur_pos, lookahead, pp.path) if lookahead is not None and mspeed is not None and ( (lookahead[1] - cur_pos[1])**2 + (lookahead[0] - cur_pos[0])**2) > 0.1: # Get heading to to lookahead from current position heading_to_lookahead = math.degrees( math.atan2(lookahead[1] - cur_pos[1], lookahead[0] - cur_pos[0])) if heading_to_lookahead < 0: heading_to_lookahead += 360 # Get difference in our heading vs heading to lookahead # Normalize error to -1 to 1 scale error = get_angle_diff(heading, heading_to_lookahead) / 180 # print(f"am at {cur_pos[0]:0.02f},{cur_pos[1]:0.02f}, want to go to {lookahead[0]:0.02f},{lookahead[1]:0.02f}") # print(f"angle delta: {error * 180:0.01f}") # print(f"error is {error}") if abs(error) < 2.0 / 180: error = 0 # Base forward velocity for both wheels forward_speed = 0.65 * (1 - abs(error))**3 # Define wheel linear velocities # Add proprtional error for turning. # TODO: PID instead of just P motor_pkt = motors() motor_pkt.left = (forward_speed - clamp(0.3 * error, -0.2, 0.2)) motor_pkt.right = (forward_speed + clamp(0.3 * error, -0.2, 0.2)) if motor_pkt.left < mspeed[0] - 0.1: motor_pkt.left = mspeed[0] - 0.1 if motor_pkt.left > mspeed[0] + 0.1: motor_pkt.left = mspeed[0] + 0.1 if motor_pkt.right < mspeed[1] - 0.1: motor_pkt.right = mspeed[1] - 0.1 if motor_pkt.right > mspeed[1] + 0.1: motor_pkt.right = mspeed[1] + 0.1 publy.publish(motor_pkt) else: # We couldn't find a suitable direction to head, stop the robot. motor_pkt = motors() motor_pkt.left = -0.25 motor_pkt.right = -0.25 publy.publish(motor_pkt)