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 steer_pub(self): servo_cmd = MotorCommand() #print self.actuator_states.motor_states[1].pulse servo_cmd.position = float( 1 * (140 - self.actuator_states.motor_states[1].pulse)) / 215 servo_cmd.joint_name = "steering" self.steering_pub.publish(servo_cmd)
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 __init__(self, sub_topic_name = '/cmd_vel'): # publisher self._drive_publisher = rospy.Publisher('/pololu/command', MotorCommand, queue_size=1) # subscriber self._sub_topic_name = sub_topic_name self._twist_subscriber = rospy.Subscriber( self._sub_topic_name, Twist_float, self.topic_callback) # get message self._drive_msg_motor = MotorCommand() self._drive_msg_steering = MotorCommand() self._twist_msg = Twist_float()
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 __init__(self): self.timeVar = 5e9 #self.didIStop = False #Publisher for sending commands to the pololu self.motor_turning_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1) self.cmd_turning = MotorCommand() self.cmd_turning.joint_name = 'front_turning' self.cmd_turning.speed = 0 self.cmd_turning.acceleration = 0 #Publisher for sending commands to the pololu self.motor_driving_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1) self.cmd_driving = MotorCommand() self.cmd_driving.joint_name = 'back_motor' self.cmd_driving.speed = 0 self.cmd_driving.acceleration = 0 #Subscriber for the PID control msgs self.sub_turning_control = rospy.Subscriber( '/turning_PID/control_effort', Float64, self.subCallback_Turning_Control, queue_size=1) #Subscriber for the PID control msgs self.sub_driving_control = rospy.Subscriber( '/driving_PID/control_effort', Float64, self.subFront_PID, queue_size=1) #Subscriber for the IMU control msgs self.sub_driving_throttle = rospy.Subscriber( '/throttle/effort', Float64, self.subCallback_Driving_Control, queue_size=1) #Subscruber for the Stop Sign Signal self.sub_stop_sign = rospy.Subscriber('/stop_sign/bool_to_stop', Bool, self.subCallback_Stop_Sign, queue_size=1) #this block is to send the desired setpoint self.setpoint_turning = 0 self.setpoint_driving = 100 self.front_previous = 0 self.stop = False self.ratio_Max = .6 self.last = rospy.get_rostime().nsecs self.stop = False self.cmd_driving.position = .15
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 __init__(self): self.joint_sub = rospy.Subscriber('/joint_states', JointState, self.joint_callback) self.head_pan_relax = rospy.ServiceProxy('/head_pan_joint/relax', Relax) self.head_tilt_relax = rospy.ServiceProxy('/head_tilt_joint/relax', Relax) self.left_elbow_relax = rospy.ServiceProxy('/left_elbow/relax', Relax) self.left_sho_pitch_relax = rospy.ServiceProxy('/left_sho_pitch/relax', Relax) self.left_sho_roll_relax = rospy.ServiceProxy('/left_sho_roll/relax', Relax) self.right_elbow_relax = rospy.ServiceProxy('/right_elbow/relax', Relax) self.right_sho_pitch_relax = rospy.ServiceProxy( '/right_sho_pitch/relax', Relax) self.right_sho_roll_relax = rospy.ServiceProxy('/right_sho_roll/relax', Relax) # Ref: https://github.com/geni-lab/hri_common/blob/master/scripts/mini_head_lip_sync_node.py self.motor_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1) self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range', MotorRange) # Setup motor command self.left_sho_roll = MotorCommand() self.left_sho_roll.joint_name = 'left_shoulder_roll_joint' self.left_sho_roll.speed = 1.0 self.left_sho_roll.acceleration = 1.0 self.left_sho_pitch = MotorCommand() self.left_sho_pitch.joint_name = 'left_shoulder_pitch_joint' self.left_sho_pitch.speed = 1.0 self.left_sho_pitch.acceleration = 1.0 self.right_sho_roll = MotorCommand() self.right_sho_roll.joint_name = 'right_shoulder_roll_joint' self.right_sho_roll.speed = 1.0 self.right_sho_roll.acceleration = 1.0 self.right_sho_pitch = MotorCommand() self.right_sho_pitch.joint_name = 'right_shoulder_pitch_joint' self.right_sho_pitch.speed = 1.0 self.right_sho_pitch.acceleration = 1.0 threading.Thread.__init__(self) self.sleeper = rospy.Rate(10)
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 __init__(self): self.timeVar = 5e9 #Publisher for sending commands to the pololu self.motor_turning_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1) self.cmd_turning = MotorCommand() self.cmd_turning.joint_name = 'front_turning' self.cmd_turning.speed = 0 self.cmd_turning.acceleration = 0 #Publisher for sending commands to the pololu self.motor_driving_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1) self.cmd_driving = MotorCommand() self.cmd_driving.joint_name = 'back_motor' self.cmd_driving.speed = 1 self.cmd_driving.acceleration = 1 #Publisher for sending the state_turning to the PID controller self.state_turning_pub = rospy.Publisher('/turning_PID/state', Float64, queue_size=1) self.state_turning = Float64() #Publisher for sending a setpoint_turning to the PID controller self.setpoint_turning_pub = rospy.Publisher('/turning_PID/setpoint', Float64, queue_size=1) self.setpoint_turning = Float64() #Publisher for sending the state_turning to the PID controller self.state_driving_pub = rospy.Publisher('/driving_PID/state', Float64, queue_size=1) self.state_driving = Float64() #Publisher for sending a setpoint_turning to the PID controller self.setpoint_driving_pub = rospy.Publisher('/driving_PID/setpoint', Float64, queue_size=1) self.setpoint_driving = Float64() #Subscriber for the state of the pololu motors self.sub_pololu = rospy.Subscriber('/pololu/motor_states', MotorStateList, self.subCallback_Turning, queue_size=1) #Subscriber for the PID control msgs self.sub_turning_control = rospy.Subscriber('/turning_PID/control_effort', Float64, self.subCallback_Turning_Control, queue_size=1) #Subscriber for the PID control msgs self.sub_driving_control = rospy.Subscriber('/driving_PID/control_effort', Float64, self.subCallback_Driving_Control, queue_size=1) self.sub_imu = rospy.Subscriber('/imu/data_raw', Imu, self.imuCallBack, queue_size=1) #this block is to send the desired setpoint self.setpoint_turning = 130 self.setpoint_driving = 120 self.front_previous = 0 self.ratio_Max = 1 self.last = rospy.get_rostime().nsecs self.stop = False self.input = [] self.output = [] self.ALPHA = 0.15
def callback_set_state(self, data): print "set_state_callback" current_bemf = data.data #motor angle speed_cmd = MotorCommand() speed_cmd.joint_name = "ESC" if (current_bemf <= self.set_state): motor_angle = .35 elif (current_bemf > self.set_state): motor_angle = .2 speed_cmd.position = motor_angle pub_esc.publish(speed_cmd)
def __init__(self): self.motor_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=1) self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range', MotorRange) # Setup motor command self.cmd = MotorCommand() self.cmd.joint_name = 'jaw_joint' self.cmd.speed = 1.0 self.cmd.acceleration = 1.0 # Add some comment here. # Get motor range rospy.loginfo('Waiting for pololu/motor_range service...') self.motor_range_srv.wait_for_service() rospy.loginfo('Found') response = self.motor_range_srv(self.cmd.joint_name) self.max_range = max([response.min, response.max]) self.sub = rospy.Subscriber('speech_strength', Float32, self.speech_strength_callback, queue_size=1)
def __init__(self, joint_name): Joint.__init__(self, joint_name) self.msg = MotorCommand() self.msg.joint_name = self.joint_name self.motor_pub = rospy.Publisher('pololu/command', MotorCommand, queue_size=10)
def publish_steering_angle(angle): cmd = MotorCommand() print("Angle: " + str(angle)) ftg_msg = Float64() ftg_msg.data = angle pub.publish(ftg_msg)
def callback(data): servo_cmd = MotorCommand() servo_cmd.joint_name = 'Servo' servo_cmd.position = (data.data * .007) pub_MotorCmd.publish(servo_cmd) motor_command = MotorCommand() motor_command.joint_name = "ESC" motor_command.position = .7 pub_MotorCmd.publish(motor_command)
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(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 callback(data): control_effort = data.data esc_cmd = MotorCommand() if(control_effort < 0): esc_cmd.position = .305 elif(control_effort >= 60): esc_cmd.position = .345 else: esc_cmd.position = .305 + control_effort/1200.0 esc_cmd.joint_name = 'ESC' pub_MotorCmd.publish(esc_cmd) '''comment me out to return steering controll''' servo_cmd = MotorCommand() servo_cmd.joint_name = 'Servo' servo_cmd.position = 0.0 pub_MotorCmd.publish(servo_cmd)
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 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 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(data): diff = data.motor_states[1].pulse - data.motor_states[2].pulse print diff servo_cmd = MotorCommand() if (diff < -100): servo_cmd.position = float(-0.6) elif (diff > 100): servo_cmd.position = 0.6 else: servo_cmd.position = diff / 167.0 servo_cmd.position = -servo_cmd.position servo_cmd.joint_name = "Servo" pub.publish(servo_cmd)
def callback(data): control_effort = data.data servo_cmd = MotorCommand() if (control_effort < -20): servo_cmd.position = float(-.6) elif (control_effort > 20): servo_cmd.position = float(.6) else: servo_cmd.position = control_effort / 33.0 servo_cmd.joint_name = "Servo" pub_servo.publish(servo_cmd)
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 __init__(self): self.motor_pub = rospy.Publisher('pololu/command',MotorCommand, queue_size=1) self.motor_range_srv = rospy.ServiceProxy('pololu/motor_range',MotorRange) # set up motor command self.cmd = MotorCommand() self.cmd.joint_name = 'ext_joint' # set motor speed self.cmd.speed = .7 self.cmd.acceleration = 0.0 # get motor range rospy.loginfo('Waiting for pololu/motor_range service...') self.motor_range_srv.wait_for_service() rospy.loginfo('Service found') response = self.motor_range_srv(self.cmd.joint_name) self.max_range = max([respone.min, response.max]) self.sub = rospy.Subscriber('ext_command',Float32, self.ext_callback, queue_size=1)
def __init__(self): self.motor_topic = '/pololu/command' self.front_topic = '/front_controller/command' self.back_topic = '/back_controller/command' self.drive_topic = '/drive/command' self.cmd_motor = MotorCommand() self.cmd_front = Float64() self.cmd_back = Float64() #create publisher passing it the vel_topic_name and msg_type self.motor_pub = rospy.Publisher(self.motor_topic, MotorCommand, queue_size=1) self.front_pub = rospy.Publisher(self.front_topic, Float64, queue_size=1) self.back_pub = rospy.Publisher(self.back_topic, Float64, queue_size=1) #create subscriber self.drive_sub = rospy.Subscriber(self.drive_topic, DriveCommand, self.scan_callback)
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)
#!/usr/bin/env python #Revision V1 11-19-19 #Updated with PID controller inital test # # # # # 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)
#!/usr/bin/env python import rospy from std_msgs.msg import String from geometry_msgs.msg import PoseStamped, Point, Twist from ros_pololu_servo.msg import MotorCommand from ros_pololu_servo.msg import HARECommand steering_cmd_msg = MotorCommand() motor_cmd_msg = MotorCommand() def cmd_cb(msg): # Limit incoming message to steering limits if msg.steering_angle > .78: msg.steering_angle = .78 elif msg.steering_angle < -.78: msg.steering_angle = -.78 steering_cmd_msg.position = msg.steering_angle # Limit the incoming throttle command if msg.throttle_cmd > 1: msg.throttle_cmd = 1 elif msg.throttle_cmd < 0: msg.throttle_cmd = 0.0 # Handle throttle mode if msg.throttle_mode == 0: motor_cmd_msg.position = 0.0 elif msg.throttle_mode == 3:
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 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)
servo_cmd.position = (data.data * .007) pub_MotorCmd.publish(servo_cmd) motor_command = MotorCommand() motor_command.joint_name = "ESC" motor_command.position = .7 pub_MotorCmd.publish(motor_command) if __name__ == '__main__': print "hello I am the servo controller" rospy.init_node("servo_control_pub", anonymous = False) pub_MotorCmd = rospy.Publisher("/pololu/command", MotorCommand, queue_size = 1) motor_setup_delay = rospy.Rate(.1) print "starting motor zero point" motor_package = MotorCommand() motor_package.joint_name = "ESC" motor_package.position = .5 pub_MotorCmd.publish(motor_package) motor_setup_delay.sleep() print "motor hold finished" rospy.Subscriber("/servo_control_effort", Float64, callback) rospy.spin()
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
from std_msgs.msg import Float32MultiArray from ros_pololu_servo.msg import MotorCommand import numpy as np import time # Set constants MAX_SPEED = 0.3 MIN_SPEED = 0 MAX_ANGLE = 0.54 SERVO_ACCELERATION = 0.04 DRIVE_ACCELERATION = 0.01 TURN_ANGLE = 0.54 ## Configure servo command channel # Servo -0.54(left) to + 0.54(right) servo_commands = MotorCommand() servo_commands.joint_name = 'servo' servo_commands.position = 0 servo_commands.speed = 0 servo_commands.acceleration = SERVO_ACCELERATION ## Configure drive motor command channel # DC Motor Min 0.05 Max 0.45 (0.62 but we don't see a change) drive_commands = MotorCommand() drive_commands.joint_name = 'motor' drive_commands.position = 0 #Set speed here drive_commands.speed = 0 drive_commands.acceleration = DRIVE_ACCELERATION s_angle = 0