Пример #1
0
    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
Пример #2
0
    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)
Пример #3
0
    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)
Пример #4
0
    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)
Пример #5
0
    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)
Пример #6
0
    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)
Пример #7
0
    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)
Пример #8
0
#!/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)