def pub_once(self):
if not self.no_speed_control:
throttle_cmd = ThrottleCmd()
throttle_cmd.enable = True
throttle_cmd.pedal_cmd_type = 2 # percentage
throttle_cmd.pedal_cmd = self._throttle
self._throttle_pub.publish(throttle_cmd)
brake_cmd = BrakeCmd()
brake_cmd.enable = True
brake_cmd.pedal_cmd_type = 2 # percentage
brake_cmd.pedal_cmd = self._brake
self._brake_pub.publish(brake_cmd)
else:
twist_cmd = TwistCmd()
#print('===== Twist_sped = {}'.format(self._twist_speed))
if self._speed_state == "GO":
twist_cmd.twist.linear.x = self._twist_speed
elif self._speed_state == "SLOW":
twist_cmd.twist.linear.x = self._twist_speed / 2.0
else: # STOP
twist_cmd.twist.linear.x = 0.0
twist_cmd.accel_limit = 1.0 # m/s^2
twist_cmd.decel_limit = 1.0 # m/s^2
self._twist_pub.publish(twist_cmd)
#self._dclient.update_configuration({"accel_ki":0.1, "accel_kp":0.4})
steer_cmd = SteeringCmd()
steer_cmd.enable = True
# rad, range -8.2 to 8.2
steer_cmd.steering_wheel_angle_cmd = self._steer
# TODO: safty, from 0 to 8.2 rad/s
steer_cmd.steering_wheel_angle_velocity = 10.0
self._steer_pub.publish(steer_cmd)
def pub_once(self):
if not self.no_speed_control:
throttle_cmd = ThrottleCmd()
throttle_cmd.enable = True
throttle_cmd.pedal_cmd_type = 2 # percentage
throttle_cmd.pedal_cmd = self._throttle
self._throttle_pub.publish(throttle_cmd)
brake_cmd = BrakeCmd()
brake_cmd.enable = True
brake_cmd.pedal_cmd_type = 2 # percentage
brake_cmd.pedal_cmd = self._brake
self._brake_pub.publish(brake_cmd)
else:
twist_cmd = TwistStamped()
print('===== Twist_sped = {}'.format(self._twist_speed))
twist_cmd.twist.linear.x = self._twist_speed
self._twist_pub.publish(twist_cmd)
steer_cmd = SteeringCmd()
steer_cmd.enable = True
# rad, range -8.2 to 8.2
steer_cmd.steering_wheel_angle_cmd = self._steer
# TODO: safty, from 0 to 8.2 rad/s
steer_cmd.steering_wheel_angle_velocity = 10.0
self._steer_pub.publish(steer_cmd)
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 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)
