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)