def cbPose(self, lane_pose_msg):
self.lane_reading = lane_pose_msg
#rospy.loginfo(self.lane_reading.d)
self.phi = self.lane_reading.phi
self.d = self.lane_reading.d #3*(self.lane_reading.d -0.125)
self.list_d.append(self.d)
self.list_d.pop(0)
self.list_phi.append(self.phi)
self.list_phi.pop(0)
avg_d = sum(self.list_d) / float(len(self.list_d))
avg_phi = sum(self.list_phi) / float(len(self.list_phi))
rospy.loginfo("D: " + str(avg_d))
rospy.loginfo("Phi: " + str(avg_phi))
# Distance between wheels
b = 0.1
# Standard velocity
gain = 0.5
v0g = 0.2
v0 = 0.145 / 0.3 * v0g
gain_p = 2
#self.s = b/2/v0 * avg_d*gain - b*avg_phi*gain
self.s = b / 2 / v0 * self.d * gain - b * self.phi * gain_p
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = header
wcs.vel_left = v0g - self.s / 2
wcs.vel_right = v0g + self.s / 2
self.pub_wheels_cmd.publish(wcs)
def cbTimer(self, event):
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = header
wcs.vel_left = 0.2 + 0.15 * math.cos(time.time() * 2 * math.pi / 2)
wcs.vel_right = 0.2 + 0.05 * math.sin(time.time() * 2 * math.pi / 2)
self.pub_wheels_cmd.publish(wcs)
def on_shutdown(self):
rospy.loginfo("[%s] Shutting down." % (self.node_name))
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
wcs = WheelsCmdStamped()
wcs.header = h
wcs.vel_left = 0
wcs.vel_right = 0
self.pub_wheels_cmd.publish(wcs)
def carCmdCallback(self, msg_car_cmd):
[d_L, d_R] = self.ik.evaluate(msg_car_cmd.omega, msg_car_cmd.v)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header = msg_car_cmd.header
msg_wheels_cmd.vel_left = d_L
msg_wheels_cmd.vel_right = d_R
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def carCmdCallback(self, msg_car_cmd):
[d_L, d_R] = self.ik.evaluate(msg_car_cmd.omega, msg_car_cmd.v)
# Put the wheel commands in a message and publish
msg_wheels_cmd = WheelsCmdStamped()
msg_wheels_cmd.header = msg_car_cmd.header
msg_wheels_cmd.vel_left = d_L
msg_wheels_cmd.vel_right = d_R
self.pub_wheels_cmd.publish(msg_wheels_cmd)
def cbDetections(self, detections_msg):
if self.too_close:
minDist = 999999999999999999999.0
offset = 0.0
# + -> offset to left in lane
# - -> offset to right in lane
for projected in detections_msg.list:
# ~0.23 is the lane width
# goes through each object- duckie or cone
# add if statement here to handle duckie different than cone
if projected.type.type == ObstacleType.DUCKIE:
stop = WheelsCmdStamped()
stop.header = bool_msg.header
stop.vel_left = 0.0
stop.vel_right = 0.0
#stop while the object is still there, and move when the coast is clear
else:
if projected.distance < minDist and abs(
projected.location.y) < 0.15:
minDist = projected.distance
y = projected.location.y
# + y -> obstacle to the left, drive right (set offset negative)
# - y -> obstacle to the right, drive left (set offset positive)
if y > 0:
offset = (-0.15 - y) * 1.5
else:
offset = (0.15 - y) * 1.5
# add else here
#Hijack the param for seting offset of the lane
rospy.set_param("lane_controller_node/d_offset", offset)
else:
#Reset offset of lane to 0
rospy.set_param("lane_controller_node/d_offset", 0.0)
# stop = WheelsCmdStamped()
# stop.header = bool_msg.header
# stop.vel_left = 0.0
# stop.vel_right = 0.0
# Slow it down so it's easier to see what's going on for now
self.lane_control.vel_left = self.lane_control.vel_left
self.lane_control.vel_right = self.lane_control.vel_right
self.pub_wheels_cmd.publish(self.lane_control)
