def steering_cmd_publisher(self, angle_rad=0.0): """ ID = 100 in BusCan # Steering Wheel float32 steering_wheel_angle_cmd # rad, range -8.2 to 8.2 float32 steering_wheel_angle_velocity # rad/s, range 0 to 8.7, 0 = maximum # Enable bool enable # Ignore driver overrides bool ignore # Disable the driver override audible warning bool quiet # Watchdog counter (optional) uint8 count """ #msg_str = "Publishing Steering message in /vehicle/steering_cmd %s" % rospy.get_time() #rospy.loginfo(msg_str) steering_command_object = SteeringCmd() #steering_command_object.steering_wheel_angle_cmd = random.uniform(-8.2, 8.2) steering_command_object.steering_wheel_angle_cmd = angle_rad steering_command_object.steering_wheel_angle_velocity = random.uniform( 0.0, 8.7) #steering_command_object.steering_wheel_angle_velocity = 0.8 steering_command_object.enable = False steering_command_object.ignore = False steering_command_object.quiet = False steering_command_object.count = 0 rospy.loginfo("Steering Publish ::>" + str(steering_command_object.steering_wheel_angle_cmd)) self.pub_steering_cmd.publish(steering_command_object)
def get_steering(self, event): if self.img is None: return message = SteeringCmd() message.enable = True message.ignore = False message.steering_wheel_angle_cmd = self.steering self.pub.publish(message)
def callback(self, data): i = 0 try: screen = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) screen = cv2.cvtColor(screen, cv2.COLOR_BGR2GRAY) screen = cv2.resize(screen, (120, 160)) screen = cv2.normalize(screen, screen, alpha=0, beta=1, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F) print "test" screen = np.array(screen).reshape(WIDTH, HEIGHT, 1) predictionx = model.predict([screen])[0] motion = predictionx[1] turn = predictionx[0] if 0.000001 < turn <= 0.01: turn1 = 0 elif turn < 0.000001: q = np.log10(turn) turn1 = q * 0.2 elif 0.01 < turn <= 0.4: turn = 1 / (turn * 10) else: turn1 = turn * 1.3 motion = motion * .15 self.pub_throttle_cmd = rospy.Publisher('/fusion/throttle_cmd', ThrottleCmd, queue_size=1) self.pub_steering_cmd = rospy.Publisher('/fusion/steering_cmd', SteeringCmd, queue_size=1) throttle_command_object = ThrottleCmd() throttle_command_object.pedal_cmd = motion throttle_command_object.pedal_cmd_type = 1 throttle_command_object.enable = True throttle_command_object.ignore = False throttle_command_object.count = 0 self.pub_throttle_cmd.publish(throttle_command_object) steering_command_object = SteeringCmd() steering_command_object.steering_wheel_angle_cmd = turn1 steering_command_object.steering_wheel_angle_velocity = random.uniform( 0.0, 8.7) steering_command_object.enable = True steering_command_object.ignore = False steering_command_object.quiet = False steering_command_object.count = 0 self.pub_steering_cmd.publish(steering_command_object) print turn, motion
def publishSteeringCmd(self, steerVal): msg = SteeringCmd() msg.steering_wheel_angle_cmd = steerVal msg.steering_wheel_angle_velocity = 0 msg.enable = True msg.clear = False msg.ignore = False msg.quiet = False msg.count = 0 self.steering_pub.publish(msg)
def make_steering_msg(angle_rad=0.0): steering_command_object = SteeringCmd() steering_command_object.steering_wheel_angle_cmd = angle_rad # steering_command_object.steering_wheel_angle_velocity = random.uniform(0.0, 8.7) steering_command_object.steering_wheel_angle_velocity = 5 * angle_rad steering_command_object.enable = True steering_command_object.ignore = False steering_command_object.quiet = False steering_command_object.count = 0 return steering_command_object
def steering_cmd_publisher(self, angle_rad=0.0): steering_command_object = SteeringCmd() steering_command_object.steering_wheel_angle_cmd = angle_rad steering_command_object.steering_wheel_angle_velocity = random.uniform(0.0, 8.7) steering_command_object.enable = True steering_command_object.ignore = False steering_command_object.quiet = False steering_command_object.count = 0 rospy.loginfo("Steering Publish::>"+str(steering_command_object.steering_wheel_angle_cmd)) rospy.loginfo("Steering wheel_angle Report::>"+str(self.steering_wheel_angle)) rospy.loginfo("Steering wheel_cmd Report::>"+str(self.steering_wheel_angle)) rospy.loginfo("Speed ::>"+str(self.speed)) self.pub_steering_cmd.publish(steering_command_object)
def steering_callback(self, steering_model_data): #print("CALLBACK") ''' The dbw_mkz SteeringCmd message looks like this: # Steering Wheel float32 steering_wheel_angle_cmd # rad, range -8.2 to 8.2 float32 steering_wheel_angle_velocity # rad/s, range 0 to 8.7, 0 = maximum # Enable bool enable # Ignore driver overrides bool ignore # Disable the driver override audible warning bool quiet # Watchdog counter (optional) uint8 count ''' # Sometimes the neural net is faster, sometimes slower. Every time it processes an input # image it will publish a steering command and that would affect the smoothing of the # values. To produce a more stable behavior, a timer is used. time_now = timer() max_update_frequency = 15 #hz time_passed = (time_now - self.last_execution_time) * 100.0 steeringCmd_msg = None if time_passed * max_update_frequency > 100.0: self.last_execution_time = time_now steeringCmd_msg = SteeringCmd() # The original steering values are in between 0 and 100 where 100 is max left and 0 is max right. # This is first smoothed over 5 previous values and then converted to the range -8.2 to 8.2 # input from the callback steering_lilliput = steering_model_data.data # The queue is emptied on the left self.previous_steering_commands.popleft() # The newest value added self.previous_steering_commands.append(steering_lilliput) # The newest steering command is the weighted sum of the previous 6 input commands # with a weighting vector, designed to change the value first by a large, later by a minor margin steering_lilliput = np.sum(self.previous_steering_commands * self.smoothing_vector) # Norm to [0,1] steering_norm = steering_lilliput / 100.0 # Regard DMZ limits steer_max_left = -8.2 # rad steer_max_right = 8.2 # rad # Observe the range of the DMZ limits ( to make it possible to change those values later ) steer_range = np.abs(steer_max_left - steer_max_right) # Map the normed steering values from lilliput to the dmz steer_output_rad = (steering_norm * steer_range) - (steer_range / 2.0) # Put it into a message steeringCmd_msg.steering_wheel_angle_cmd = steer_output_rad # Fill other fields steeringCmd_msg.steering_wheel_angle_velocity = 1.0 steeringCmd_msg.enable = True steeringCmd_msg.ignore = False steeringCmd_msg.quiet = False # If the watchdog counter should be increased do so if (self.watchdog_counter_used): steeringCmd_msg.count = self.counter # If the watchdog counter rises to quickly there is # another trick here to make him rise slower if (self.watchdog_throttle): if (np.random() > self.throttle_value): self.counter = self.counter + 1 else: self.counter = self.counter + 1 self.steeringCmd_msg = steeringCmd_msg # The message has to be sent at a higher rate than # the net is working so that rate is realized here #if not self.thread_started: # threading.Thread(target=self.sendMessageAtRate).start() # self.thread_started = True self.steer_pub.publish(self.steeringCmd_msg)