def check_crashed(data): # Check whether we have crashed or not if not data.data: # Drive straight until we hit something msg = MotorCommand() msg.joint_name = "steering" msg.acceleration = 0 msg.speed = 1 msg.position = STEER_STRAIGHT pololu_pub.publish(msg) msg = MotorCommand() msg.joint_name = "drive" msg.acceleration = 0 msg.speed = 1 msg.position = CRASH_VELOCITY pololu_pub.publish(msg) else: # Make sure to turn off crash mode crash_mode = False reverse_reverse() return
def reverse_reverse(): # But first stop msg = MotorCommand() msg.joint_name = "drive" msg.acceleration = 0 msg.speed = 1 msg.position = DRIVE_STOP pololu_pub.publish(msg) msg = MotorCommand() msg.joint_name = "steering" msg.acceleration = 0 msg.speed = 1 msg.position = STEER_STRAIGHT pololu_pub.publish(msg) # Stop for 1 second and reflect on what you have done time.sleep(1) # Now reverse reverse msg = MotorCommand() msg.joint_name = "drive" msg.acceleration = 0 msg.speed = 1 msg.position = BACKUP_VELOCITY pololu_pub.publish(msg) # Put processes to sleep for 3 seconds time.sleep(3) # Now go back to f*****g shit up return
def callback(self,msg): mtr=MotorCommand() mtr.joint_name=self.channel_name if msg.data==True: mtr.position=0.77 mtr.speed=1.0 mtr.acceleration=1.0 else: mtr.position=-0.77 mtr.speed=0.0 mtr.acceleration=0.0 self.pub.publish(mtr)
def moveMotor(self, jntName, pos, speed): mtr = MotorCommand() mtr.joint_name = jntName mtr.position = pos mtr.speed = speed #/self.MaxSpeed#pololu take 0 to 1.0 as speed, check the correct division mtr.acceleration = 1.0 self.pub.publish(mtr)
def moveMotor(self,jntName,pos,speed): mtr=MotorCommand() mtr.joint_name=jntName mtr.position=pos mtr.speed=speed#/self.MaxSpeed#pololu take 0 to 1.0 as speed, check the correct division mtr.acceleration=1.0 self.pub.publish(mtr)
def main(): rospy.init_node('killing_motor') motor_pub = rospy.Publisher('/pololu/command/', MC, queue_size=3) output = MC() output.joint_name = 'Throttle' output.position = 0 output.speed = 1.0 output.acceleration = 1 motor_pub.publish(output)
def start_expression(self, goal_handle): goal = goal_handle.get_goal() expression = Expression[goal.expression] speed = goal.speed intensity = goal.intensity duration = goal.duration # Checking for default values (-1 means nothing was specified, hence we should use the default values) if speed == -1: speed = expression.default_speed if intensity == -1: intensity = expression.default_intensity if duration == -1 and expression.default_duration is not None: duration = expression.default_duration if expression in [Expression.smile, Expression.frown_mouth]: joint_name = 'smile_joint' elif expression in [Expression.open_mouth]: joint_name = 'jaw_joint' elif expression in [Expression.frown]: joint_name = 'brow_joint' negative = True if expression in [Expression.smile, Expression.open_mouth]: negative = False response = self.motor_range_srv(joint_name) msg = MotorCommand() msg.joint_name = joint_name min_range = min([response.min, response.max]) max_range = max([response.min, response.max]) if negative: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range) else: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range) msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5) msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4) start = time.time() while not rospy.is_shutdown( ) and not self.action_server.is_preempt_requested(goal_handle) and ( time.time() - start) < duration: if duration >= 0.0 and (time.time() - start) < duration: break self.motor_pub.publish(msg) self.rate.sleep() if not self.action_server.is_preempt_requested(goal_handle): self.action_server.set_succeeded(goal_handle)
def callback_states(self, states): self.actuator_states = states left_ir = states.motor_states[0].pulse right_ir = states.motor_states[1].pulse d_ref = 142 #d_ref = 95 #d_ref = 120 print states.motor_states[1].pulse # Publish left IR state : distance from the wall self.distance_pub_r.publish(states.motor_states[1].pulse) self.distance_pub_l.publish(states.motor_states[0].pulse) servo_cmd = MotorCommand() self.current_time = rospy.Time.now() #self.dt = (self.current_time - self.last_time).to_sec() error = left_ir - right_ir self.error_pub.publish(error) if error < -2: u = float(self.Kp) * error / 5.58 elif error > 2: u = float(self.Kp) * error / 25.67 else: u = 0 if u > 3: u = 2.2 elif u < -17: u = -17 # Publish steering control effort self.controlEffort_pub.publish(u) #print u servo_cmd.position = u servo_cmd.joint_name = "steering" self.steering_pub.publish(servo_cmd) servo_cmd.joint_name = "thrust" servo_cmd.speed = float(0) servo_cmd.acceleration = float(0) if self.stop_detected == True: servo_cmd.position = float(0) else: servo_cmd.position = float(0.48) #servo_cmd.position = float(0) self.steering_pub.publish(servo_cmd) self.last_time = self.current_time self.last_error = error
def start_expression(self, goal_handle): goal = goal_handle.get_goal() expression = Expression[goal.expression] speed = goal.speed intensity = goal.intensity duration = goal.duration # Checking for default values (-1 means nothing was specified, hence we should use the default values) if speed == -1: speed = expression.default_speed if intensity == -1: intensity = expression.default_intensity if duration == -1 and expression.default_duration is not None: duration = expression.default_duration if expression in [Expression.smile, Expression.frown_mouth]: joint_name = 'smile_joint' elif expression in [Expression.open_mouth]: joint_name = 'jaw_joint' elif expression in [Expression.frown]: joint_name = 'brow_joint' negative = True if expression in [Expression.smile, Expression.open_mouth]: negative = False response = self.motor_range_srv(joint_name) msg = MotorCommand() msg.joint_name = joint_name min_range = min([response.min, response.max]) max_range = max([response.min, response.max]) if negative: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range) else: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range) msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5) msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4) start = time.time() while not rospy.is_shutdown() and not self.action_server.is_preempt_requested(goal_handle) and (time.time() - start) < duration: if duration >= 0.0 and (time.time() - start) < duration: break self.motor_pub.publish(msg) self.rate.sleep() if not self.action_server.is_preempt_requested(goal_handle): self.action_server.set_succeeded(goal_handle)
def listen(self, msg): '''string joint_name # Name of the joint (specified in the yaml file), or motor_id for default calibration float64 position # Position to move to in radians float32 speed # Speed to move at (0.0 - 1.0) float32 acceleration # Acceleration to move at (0.0 - 1.0)''' output = MC() output.joint_name = 'Turning' output.position = msg.data output.speed = 0.5 output.acceleration = 0.1 self.motor_pub.publish(output)
def actuate(ang, ang_speed, ang_acc, vel, vel_speed, vel_acc): #cmd.position: radians, -45 min (left), -45 max (right) #speed and accel 0 to 1 cmd = MotorCommand() cmd.joint_name = "steering" #cmd.position = degToRad(ang) cmd.position = ang cmd.speed = ang_speed cmd.acceleration = ang_acc print("Setting angle to ", ang) pololu_pub.publish(cmd) #cmd.position: radians, 0 min, 45 max (1560) #speed and accel 0 to 1 cmd.joint_name = "drive" #cmd.position = degToRad(vel) cmd.position = vel cmd.speed = vel_speed cmd.acceleration = vel_acc print("Setting speed to ", vel) pololu_pub.publish(cmd)
def throttle_listen(self, msg): '''string joint_name # Name of the joint (specified in the yaml file), or motor_id for default calibration float64 position # Position to move to in radians float32 speed # Speed to move at (0.0 - 1.0) float32 acceleration # Acceleration to move at (0.0 - 1.0)''' if abs(self.throttle_last - msg.data) / (abs(msg.data) + 0.001) > PUBTHRESH: output = MC() output.joint_name = 'Throttle' output.position = msg.data output.speed = 1.0 output.acceleration = 0.5 self.motor_pub.publish(output) self.throttle_last = msg.data
def start_expression(self, goal_handle): goal = goal_handle.get_goal() expression = ZoidExpression[goal.expression] speed = goal.speed intensity = goal.intensity duration = goal.duration if expression in [ZoidExpression.smile, ZoidExpression.frown_mouth]: joint_name = 'smile_joint' elif expression in [ZoidExpression.open_mouth]: joint_name = 'jaw_joint' elif expression in [ZoidExpression.frown]: joint_name = 'brow_joint' negative = True if expression in [ZoidExpression.smile, ZoidExpression.open_mouth]: negative = False response = self.motor_range_srv(joint_name) msg = MotorCommand() msg.joint_name = joint_name min_range = min([response.min, response.max]) max_range = max([response.min, response.max]) if negative: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range) else: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range) msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5) msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4) start = time.time() while not rospy.is_shutdown( ) and not self.action_server.is_preempt_requested(goal_handle) and ( time.time() - start) < duration: self.motor_pub.publish(msg) self.rate.sleep() if not self.action_server.is_preempt_requested(goal_handle): self.action_server.set_succeeded(goal_handle)
def start_expression(self, goal_handle): goal = goal_handle.get_goal() expression = ZoidExpression[goal.expression] speed = goal.speed intensity = goal.intensity duration = goal.duration if expression in [ZoidExpression.smile, ZoidExpression.frown_mouth]: joint_name = 'smile_joint' elif expression in [ZoidExpression.open_mouth]: joint_name = 'jaw_joint' elif expression in [ZoidExpression.frown]: joint_name = 'brow_joint' negative = True if expression in [ZoidExpression.smile, ZoidExpression.open_mouth]: negative = False response = self.motor_range_srv(joint_name) msg = MotorCommand() msg.joint_name = joint_name min_range = min([response.min, response.max]) max_range = max([response.min, response.max]) if negative: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, min_range) else: msg.position = interpolate(intensity, 0.0, 1.0, 0.0, max_range) msg.speed = interpolate(speed, 0.0, 1.0, 0.0, 0.5) msg.acceleration = interpolate(speed, 0.0, 1.0, 0.0, 0.4) start = time.time() while not rospy.is_shutdown() and not self.action_server.is_preempt_requested(goal_handle) and (time.time() - start) < duration: self.motor_pub.publish(msg) self.rate.sleep() if not self.action_server.is_preempt_requested(goal_handle): self.action_server.set_succeeded(goal_handle)
# # import rospy from std_msgs.msg import Float32MultiArray # from ackermann_msgs.msg import AckermannDriveStamped from ros_pololu_servo.msg import MotorCommand # from sensor_msgs.msg import LaserScan import numpy as np import time servo_commands = MotorCommand() drive_commands = MotorCommand() # DC Motor Min 0.1 Max 0.45 (0.62 but we don't see a change) servo_commands.joint_name = 'servo' #Check name here drive_commands.joint_name = 'motor' #Check name here drive_commands.position = 0 #Set speed here drive_commands.speed = 0 # acceleration for best run = .2 drive_commands.acceleration = .07 servo_commands.position = 0 servo_commands.speed = 0.2 servo_commands.acceleration = 0.05 turn_number = 0 s_angle = 0 pub = rospy.Publisher('/pololu/command', MotorCommand, queue_size=1) #pub.publish(drive_commands) raw_input("Press Enter to RUMBLEEEE") #State Machine Variable car_state = 1 max_speed = .3
def test(data): # Check if we want to crash and backout if crash_mode: return pub_gap = rospy.Publisher('ftg', LaserScan, queue_size=1) pub_no_gap = rospy.Publisher('noGap', Bool, queue_size=10) # It would be good here to package this angle information into the message left_ang = np.rad2deg(data.angle_max) right_ang = np.rad2deg(data.angle_max) inc_ang = np.rad2deg(data.angle_increment) depths = data.ranges depth_thresh = DEPTH_THRESH depths = list(depths) for idx, depth in enumerate(depths): if depth > depth_thresh: depths[idx] = 0 c_depths = copy.deepcopy(depths) start = 0 num_zeros = 0 in_gap = False cur_zeros = 0 cur_start = 0 for idx, depth in enumerate(depths): if not depth == 0: depths[idx] = 1 cur_zeros = 0 in_gap = False else: if not in_gap: cur_start = idx cur_zeros = 0 in_gap = True depths[idx] = 0.5 cur_zeros += 1 if cur_zeros > num_zeros: num_zeros = cur_zeros start = cur_start middle = start + num_zeros/2 ang = -(45.0 - inc_ang * middle) c_depths_2 = copy.deepcopy(depths) c_start = 0 in_gap = False cur_zeros = 0 cur_start = 0 window_size = 20 sum_max = 0 cur_sum = 0 mid_idx = 0 go_far = False section_len = 100 # check if no gap big enough exists if num_zeros < 150: go_far = True for idx, depth in enumerate(c_depths_2): if idx - section_len < len(c_depths_2): cur_sum = sum(c_depths_2[idx:idx + section_len - 1]) if cur_sum < sum_max: sum_max = cur_sum mid_idx = idx + int(section_len/2) print("Go at the furthest object!") no_pub = False if go_far: frac = mid_idx/float(len(c_depths)) frac = frac * 90.0 ang = -(frac - 45.0) #ang = ang - (0.2 * ang) if ang < 0: if Clockwise: ang = -45.0 else: ang = 45.0 else: if Clockwise: ang = 45.0 else: ang = -45.0 real_ang = ang print('Angle', ang, 'start', start, 'num zeros', num_zeros) msg = MotorCommand() msg.joint_name = "drive" msg.acceleration = 0 msg.speed = 1 if go_far: msg.position = VELOCITY/2.0 else: msg.position = VELOCITY pololu_pub.publish(msg) msg.joint_name = "steering" if Clockwise: ang = (((ang + 45.0)/90.0) * 2.0) - 1.0 else: ang = (((ang + 45.0)/90.0) * 2.0) - 1.0 + 0.2 msg.acceleration = ACC msg.speed = ACC msg.position = ang if not no_pub: pololu_pub.publish(msg)