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 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)
