def throttle_cmd_publisher(self, throttle_value=0.0):
"""
ID = 98 in BUSCAN
# Throttle pedal
# Options defined below
float32 pedal_cmd
uint8 pedal_cmd_type
# Enable
bool enable
# Ignore driver overrides
bool ignore
# Watchdog counter (optional)
uint8 count
uint8 CMD_NONE=0
uint8 CMD_PEDAL=1 # Unitless, range 0.15 to 0.80
uint8 CMD_PERCENT=2 # Percent of maximum throttle, range 0 to 1
"""
#msg_str = "Publishing Throttle message in /vehicle/throttle_cmd %s" % rospy.get_time()
#rospy.loginfo(msg_str)
throttle_command_object = ThrottleCmd()
#throttle_command_object.pedal_cmd = random.uniform(0.15, 0.8)
throttle_command_object.pedal_cmd = throttle_value
throttle_command_object.pedal_cmd_type = 1
throttle_command_object.enable = False
throttle_command_object.ignore = False
throttle_command_object.count = 0
rospy.loginfo("Throttle Publish ::>" +
str(throttle_command_object.pedal_cmd))
self.pub_throttle_cmd.publish(throttle_command_object)
def publishThrottleCmd(self, pedalVal):
msg = ThrottleCmd()
msg.pedal_cmd = pedalVal
msg.pedal_cmd_type = 2
msg.enable = True
msg.clear = False
msg.ignore = False
msg.count = 0
self.throttle_pub.publish(msg)
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 make_throttle_msg(throttle_value=0.0):
throttle_command_object = ThrottleCmd()
throttle_command_object.pedal_cmd = throttle_value
throttle_command_object.pedal_cmd_type = 2
throttle_command_object.enable = True
throttle_command_object.ignore = False
throttle_command_object.count = 0
return throttle_command_object
def throttle_cmd_publisher(self, throttle_value=0.0):
throttle_command_object = ThrottleCmd()
throttle_command_object.pedal_cmd = throttle_value
throttle_command_object.pedal_cmd_type = 1
throttle_command_object.enable = True
throttle_command_object.ignore = False
throttle_command_object.count = 0
rospy.loginfo("Throttle Publish ::>"+str(throttle_command_object.pedal_cmd))
rospy.loginfo("Throttle Pedal_cmd Report ::>"+str(self.throttle_pedal_cmd))
rospy.loginfo("Throttle Pedal_input Report ::>"+str(self.throttle_pedal_input))
rospy.loginfo("Throttle Pedal_output Report ::>"+str(self.throttle_pedal_output))
self.pub_throttle_cmd.publish(throttle_command_object)
def publishThrottleCmd(self, pedalVal): msg = ThrottleCmd() msg.pedal_cmd = pedalVal msg.pedal_cmd_type = 2 ''' pedal_cmd_type=1: Unitless, range 0.15 to 0.50 =2: Percent of maximum throttle, range 0 to 1 ''' msg.enable = True msg.clear = False msg.ignore = False msg.count = 0 self.throttle_pub.publish(msg)
