def publishControl(self, event):
# Don't publish if having received any Joy msgs
if self.joy is None:
if not self.has_complained:
rospy.loginfo("[%s] Has not received Joy msg yet. Not publishing." %(self.node_name))
self.has_complained = True
return
speed = self.joy.axes[1] * self.speed_gain #Left stick V-axis. Up is positive
steering = self.joy.axes[3] * self.steer_gain
# do not publish if values did not change
if self.last_pub_msg is not None:
same = (self.last_pub_msg.speed == speed) and (self.last_pub_msg.steering == steering)
# if it's same, publish only every 2 seconds
if same:
delta = event.current_real - self.last_pub_msg.header.stamp
if delta.to_sec() < 2.0:
return
car_control_msg = CarControl()
car_control_msg.need_steering = False
car_control_msg.speed = speed
car_control_msg.steering = steering #*self.steer_gain #Right stick H-axis. Right is negative
car_control_msg.header.stamp = self.joy.header.stamp
rospy.loginfo("[%s] controls: speed %f, steering %f" % (self.node_name,car_control_msg.speed, car_control_msg.steering))
self.pub_control.publish(car_control_msg)
self.last_pub_msg = car_control_msg
def custom_shutdown(self):
rospy.loginfo("[%s] Shutting down..." % self.node_name)
# Stop listening
self.sub_lane_reading.unregister()
# Send stop command
car_control_msg = CarControl()
car_control_msg.need_steering = True
car_control_msg.speed = 0.0
car_control_msg.steering = 0.0
# self.pub_lane_cmd.publish(car_control_msg)
rospy.sleep(0.5) #To make sure that it gets published.
rospy.loginfo("[%s] Shutdown" % self.node_name)
def custom_shutdown(self):
rospy.loginfo("[%s] Shutting down..." %self.node_name)
# Stop listening
self.sub_lane_reading.unregister()
# Send stop command
car_control_msg = CarControl()
car_control_msg.need_steering = True
car_control_msg.speed = 0.0
car_control_msg.steering = 0.0
self.pub_control.publish(car_control_msg)
rospy.sleep(0.5) #To make sure that it gets published.
rospy.loginfo("[%s] Shutdown" %self.node_name)
def cbPose(self,msg):
self.lane_reading = msg
cross_track_err = msg.y
heading_err = msg.phi
car_control_msg = CarControl()
car_control_msg.need_steering = True
car_control_msg.speed = self.v_bar #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(cross_track_err) > self.d_thres:
cross_track_err = cross_track_err / math.fabs(cross_track_err) * self.d_thres
car_control_msg.steering = self.k_d * cross_track_err + self.k_theta * heading_err #*self.steer_gain #Right stick H-axis. Right is negative
# controller mapping issue
# car_control_msg.steering = -car_control_msg.steering
# print "controls: speed %f, steering %f" % (car_control_msg.speed, car_control_msg.steering)
self.pub_control.publish(car_control_msg)
def publishCmd(self,stamp,speed,steering):
lane_cmd_msg = CarControl()
lane_cmd_msg.header.stamp = stamp
lane_cmd_msg.speed = speed
lane_cmd_msg.steering = steering
lane_cmd_msg.need_steering = True
wheels_cmd_msg = WheelsCmd()
# wheels_cmd_msg.header.stamp = stamp
ratio = 1.0
gain = 0.5
vel_left = gain*(speed - steering)*ratio
vel_right = gain*(speed + steering)*(1.0/ratio)
wheels_cmd_msg.vel_left = np.clip(vel_left,-1.0,1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right,-1.0,1.0)
self.pub_lane_cmd.publish(lane_cmd_msg)
self.pub_wheels_cmd.publish(wheels_cmd_msg)
def publishCmd(self, stamp, speed, steering):
lane_cmd_msg = CarControl()
lane_cmd_msg.header.stamp = stamp
lane_cmd_msg.speed = speed
lane_cmd_msg.steering = steering
lane_cmd_msg.need_steering = True
wheels_cmd_msg = WheelsCmd()
# wheels_cmd_msg.header.stamp = stamp
ratio = 1.0
gain = 0.5
vel_left = gain * (speed - steering) * ratio
vel_right = gain * (speed + steering) * (1.0 / ratio)
wheels_cmd_msg.vel_left = np.clip(vel_left, -1.0, 1.0)
wheels_cmd_msg.vel_right = np.clip(vel_right, -1.0, 1.0)
self.pub_lane_cmd.publish(lane_cmd_msg)
self.pub_wheels_cmd.publish(wheels_cmd_msg)
def cbPose(self, msg):
self.lane_reading = msg
cross_track_err = msg.d
heading_err = msg.phi
car_control_msg = CarControl()
car_control_msg.need_steering = True
car_control_msg.speed = self.v_bar #*self.speed_gain #Left stick V-axis. Up is positive
if math.fabs(cross_track_err) > self.d_thres:
cross_track_err = cross_track_err / math.fabs(
cross_track_err) * self.d_thres
car_control_msg.steering = self.k_d * cross_track_err + self.k_theta * heading_err #*self.steer_gain #Right stick H-axis. Right is negative
# controller mapping issue
# car_control_msg.steering = -car_control_msg.steering
# print "controls: speed %f, steering %f" % (car_control_msg.speed, car_control_msg.steering)
# self.pub_.publish(car_control_msg)
self.publishCmd(msg.header.stamp, self.v_bar, car_control_msg.steering)
#!/usr/bin/env python
import rospy
from duckietown_msgs.msg import CarControl
if __name__ == '__main__':
# Initialize the node with rospy
rospy.init_node('dagu_car_tester', anonymous=False)
pub = rospy.Publisher("~car_control",CarControl,queue_size=1)
rospy.loginfo("[dagu_car_tester] Initializing.")
rospy.sleep(1.0)
# Full speed ahead
pub.publish(CarControl(speed=1.0,steering=0.0))
rospy.loginfo("[dagu_car_tester] Full speed ahead.")
rospy.sleep(3.0)
# Full speed reverse
pub.publish(CarControl(speed=-1.0,steering=0.0))
rospy.loginfo("[dagu_car_tester] Full speed reverse")
rospy.sleep(3.0)
# Full speed left turn
pub.publish(CarControl(speed=1.0,steering=1.0))
rospy.loginfo("[dagu_car_tester] Full speed left turn")
rospy.sleep(3.0)
# Full speed left turn
pub.publish(CarControl(speed=1.0,steering=-1.0))
rospy.loginfo("[dagu_car_tester] Full speed right turn")
rospy.sleep(3.0)
# Stop
pub.publish(CarControl(speed=0.0,steering=0.0))
rospy.loginfo("[dagu_car_tester] Stop")
rospy.sleep(3.0)
