예제 #1
0
 def cbWheelsCmd(self, msg):
     trimmed_cmd = WheelsCmd()
     trimmed_cmd.vel_left = np.clip(msg.vel_left * (1.0 - self.trim), -1.0,
                                    1.0)
     trimmed_cmd.vel_right = np.clip(msg.vel_right * (1.0 + self.trim),
                                     -1.0, 1.0)
     self.pub_topic.publish(trimmed_cmd)
예제 #2
0
    def publishControl(self):
        speed = self.joy.axes[1] * self.speed_gain #Left stick V-axis. Up is positive
        steering = self.joy.axes[3] * self.steer_gain
        wheels_cmd_msg = WheelsCmd()

        # Car Steering Mode
        vel_left = (speed - steering)
        vel_right = (speed + steering)
        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)
        rospy.loginfo("[%s] left %f, right %f" % (self.node_name,wheels_cmd_msg.vel_left,wheels_cmd_msg.vel_right))
        self.pub_wheels.publish(wheels_cmd_msg)
예제 #3
0
    def publishControl(self):
        speed = self.joy.axes[
            1] * self.speed_gain  #Left stick V-axis. Up is positive
        steering = self.joy.axes[3] * self.steer_gain
        wheels_cmd_msg = WheelsCmd()

        # Car Steering Mode
        vel_left = (speed - steering)
        vel_right = (speed + steering)
        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)
        rospy.loginfo("[%s] left %f, right %f" %
                      (self.node_name, wheels_cmd_msg.vel_left,
                       wheels_cmd_msg.vel_right))
        self.pub_wheels.publish(wheels_cmd_msg)
예제 #4
0
    def __init__(self):
        # Save the name of the node
        self.node_name = rospy.get_name()
        
        rospy.loginfo("[%s] Initialzing." %(self.node_name))

        # Setup publishers
        self.pub_topic_a = rospy.Publisher("~topic_a",String, queue_size=1)
        self.pub_wheels_cmd = rospy.Publisher("/kitt/wheels_driver/wheels_cmd",WheelsCmd, queue_size=1)
        # Setup subscriber
        self.sub_topic_b = rospy.Subscriber("~topic_b", String, self.cbTopic)
        # Read parameters
        self.pub_timestep = self.setupParameter("~pub_timestep",1.0)
        # Create a timer that calls the cbTimer function every 1.0 second
        self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.cbTimer)

        rospy.loginfo("[%s] Initialzed." %(self.node_name))

        rate = rospy.Rate(30) # 10hz
        #move forward
        forward_for_time_leave = 2.25
        turn_for_time = 0.75
        forward_for_time_enter = 1
        
        starting_time = rospy.Time.now()
        while((rospy.Time.now() - starting_time) < rospy.Duration(forward_for_time_leave)):
            wheels_cmd_msg = WheelsCmd()
            wheels_cmd_msg.vel_left = 0.4
            wheels_cmd_msg.vel_right = 0.4
            self.pub_wheels_cmd.publish(wheels_cmd_msg)    
            rospy.loginfo("Moving?.")
            rate.sleep()
        #turn right
        starting_time = rospy.Time.now()
        while((rospy.Time.now() - starting_time) < rospy.Duration(turn_for_time)):
            wheels_cmd_msg = WheelsCmd()
            wheels_cmd_msg.vel_left = 0.5
            wheels_cmd_msg.vel_right = -0.5
            self.pub_wheels_cmd.publish(wheels_cmd_msg)    
            rospy.loginfo("Moving?.")
            rate.sleep()
   
        #move forward
        starting_time = rospy.Time.now()
        while((rospy.Time.now() - starting_time) < rospy.Duration(forward_for_time_enter)):
            wheels_cmd_msg = WheelsCmd()
            wheels_cmd_msg.vel_left = 0.4
            wheels_cmd_msg.vel_right = 0.4
            self.pub_wheels_cmd.publish(wheels_cmd_msg)    
            rospy.loginfo("Moving?.")
            rate.sleep()
예제 #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 cbWheelsCmd(self,msg):
     trimmed_cmd = WheelsCmd()
     trimmed_cmd.vel_left = np.clip(msg.vel_left*(1.0-self.trim),-1.0,1.0)
     trimmed_cmd.vel_right = np.clip(msg.vel_right*(1.0+self.trim),-1.0,1.0)
     self.pub_topic.publish(trimmed_cmd)
예제 #8
0
    def __init__(self):
        # Save the name of the node
        self.node_name = rospy.get_name()

        rospy.loginfo("[%s] Initialzing." % (self.node_name))

        # Setup publishers
        self.pub_topic_a = rospy.Publisher("~topic_a", String, queue_size=1)
        self.pub_wheels_cmd = rospy.Publisher("/kitt/wheels_driver/wheels_cmd",
                                              WheelsCmd,
                                              queue_size=1)
        # Setup subscriber
        self.sub_topic_b = rospy.Subscriber("~topic_b", String, self.cbTopic)
        # Read parameters
        self.pub_timestep = self.setupParameter("~pub_timestep", 1.0)
        # Create a timer that calls the cbTimer function every 1.0 second
        self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),
                                 self.cbTimer)

        rospy.loginfo("[%s] Initialzed." % (self.node_name))

        rate = rospy.Rate(30)  # 10hz
        #move forward
        forward_for_time_leave = 2.25
        turn_for_time = 0.75
        forward_for_time_enter = 1

        starting_time = rospy.Time.now()
        while ((rospy.Time.now() - starting_time) <
               rospy.Duration(forward_for_time_leave)):
            wheels_cmd_msg = WheelsCmd()
            wheels_cmd_msg.vel_left = 0.4
            wheels_cmd_msg.vel_right = 0.4
            self.pub_wheels_cmd.publish(wheels_cmd_msg)
            rospy.loginfo("Moving?.")
            rate.sleep()
        #turn right
        starting_time = rospy.Time.now()
        while ((rospy.Time.now() - starting_time) <
               rospy.Duration(turn_for_time)):
            wheels_cmd_msg = WheelsCmd()
            wheels_cmd_msg.vel_left = 0.5
            wheels_cmd_msg.vel_right = -0.5
            self.pub_wheels_cmd.publish(wheels_cmd_msg)
            rospy.loginfo("Moving?.")
            rate.sleep()

        #move forward
        starting_time = rospy.Time.now()
        while ((rospy.Time.now() - starting_time) <
               rospy.Duration(forward_for_time_enter)):
            wheels_cmd_msg = WheelsCmd()
            wheels_cmd_msg.vel_left = 0.4
            wheels_cmd_msg.vel_right = 0.4
            self.pub_wheels_cmd.publish(wheels_cmd_msg)
            rospy.loginfo("Moving?.")
            rate.sleep()