def move_forward_controlled(self, speed, distance, update_rate): # Initialize the movement command move_cmd = AckermannDrive() # Set the forward speed # Setting no acceleration or jerk will accelerate at maximum rate move_cmd.speed = speed # Continue to move until we get close to the mark error = distance while abs(error) > self.tolerance: # If we have not achieved our goal, move in the appropriate direction move_cmd.speed = math.copysign(self.speed, -1 * error) self.cmd_ack.publish(move_cmd) self.rate.sleep() # Get the current position from the tf transform between the odom and base frames self.position = self.get_position() # Compute the Euclidean distance from the target point distance_travelled = math.sqrt( pow((self.position.x - self.x_start), 2) + pow((self.position.y - self.y_start), 2)) # How close are we? error = distance_travelled - distance # Stop the robot before the next leg self.cmd_ack.publish(AckermannDrive()) rospy.sleep(1)
def drive_straight(self): while not rospy.is_shutdown(): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = 2.0 drive_msg.steering_angle = 0.0 drive_msg.acceleration = 0 drive_msg.jerk = 0 drive_msg.steering_angle_velocity = 0 drive_msg_stamped.drive = drive_msg self.pub.publish(drive_msg_stamped) if (SPEED == 0): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = 0.0 drive_msg.steering_angle = 0.0 drive_msg.acceleration = 0 drive_msg.jerk = 0 drive_msg.steering_angle_velocity = 0 drive_msg_stamped.drive = drive_msg self.pub.publish(drive_msg_stamped) rospy.sleep(5000) # don't spin too fast rospy.sleep(1.0 / PUBLISH_RATE)
def processError(self, error, dist): if dist == 0: steering_angle_cmd = 0 speed_cmd = 0 else: error = error / 1000.0 steering_angle_cmd = self.pid.update(error) if dist < SLOWDOWN_DISTANCE: speed_cmd = self.speed_slow else: speed_cmd = self.speed if not self.sim: speed_cmd += abs( self.latest_trailer_angle ) * SPEED_INCREASE_COEFFICIENT / MAX_TRAILER_ANGLE ack = AckermannDrive() ack.steering_angle = steering_angle_cmd ack.speed = speed_cmd if self.lock_stop: ack.speed = 0 self.drive_publisher.publish(ack)
def rearWheelFeedback(self, currentPose, targetPose): #Gain Values k1 = 4 k2 = 15 k3 = 4 #give targetVel and targetAngVel targetVel = math.sqrt( (targetPose.twist.linear.x * targetPose.twist.linear.x) + ((targetPose.twist.linear.y * targetPose.twist.linear.y))) targetAngVel = targetPose.twist.angular.z #print (targetVel, targetAngVel) currentEuler = self.quaternion_to_euler(currentPose.pose.orientation.x, currentPose.pose.orientation.y, currentPose.pose.orientation.z, currentPose.pose.orientation.w) targetEuler = self.quaternion_to_euler(targetPose.pose.orientation.x, targetPose.pose.orientation.y, targetPose.pose.orientation.z, targetPose.pose.orientation.w) #compute errors xError = ( (targetPose.pose.position.x - currentPose.pose.position.x) * math.cos(currentEuler[2])) + ( (targetPose.pose.position.y - currentPose.pose.position.y) * math.sin(currentEuler[2])) yError = ( (targetPose.pose.position.x - currentPose.pose.position.x) * math.sin(currentEuler[2]) * -1) + ( (targetPose.pose.position.y - currentPose.pose.position.y) * math.cos(currentEuler[2])) thetaError = targetEuler[2] - currentEuler[2] #print("Error:", xError, yError, thetaError) #give blank ackermannCmd newAckermannCmd = AckermannDrive() if (2 * math.cos(thetaError)) + (k1 * xError) <= 2: newAckermannCmd.speed = (2 * math.cos(thetaError)) + (k1 * xError) else: newAckermannCmd.speed = 2 newAckermannCmd.steering_angle_velocity = (0) + ((2) * ( (k2 * yError) + (k3 * math.sin(thetaError)))) #print(newAckermannCmd) return newAckermannCmd
def odom_callback(msg): ack_msg = AckermannDrive() ack_msg_stmp = AckermannDriveStamped() #Speed: Set constant half max speed ack_msg.speed = max_speed / 2.0 #Steering: Random btwn 0 and 1 rand = random() steering_range = max_steering_angle / 2.0 rand_angle = steering_range * rand - steering_range / 2.0 # sometimes change sign so it turns more (basically add bias to continue turning in current direction) rand = random() if rand > 0.8 and prev_angle != 0.0: sign_rand = rand_angle / abs(rand_angle) sign_prev = prev_angle / abs(prev_angle) rand_angle *= sign_rand * sign_prev #set angle ack_msg.steering_angle = min(max(prev_angle + rand_ang, -max_steering_angle), max_steering_angle)) #reset prev_angle prev_angle = ack_msg.steering_angle ack_msg_stmp = ack_msg drive_pub.publish(ack_msg_stmp)
def joy_command_callback(data): global current_command_topic global offboard_command passthrough_command = AckermannDrive() # listen to control transfer commands if data.buttons[ctl_offboard_button] and not data.buttons[ctl_teleop_button]: if not message_display[0]: rospy.loginfo(log_message.format(control_priority[0])) message_display[0] = True message_display[1] = False if current_command_topic != command_topic[0]: current_command_topic = command_topic[0] if not data.buttons[ctl_offboard_button] and data.buttons[ctl_teleop_button]: if not message_display[1]: rospy.loginfo(log_message.format(control_priority[1])) message_display[0] = False message_display[1] = True if current_command_topic != command_topic[1]: current_command_topic = command_topic[1] if current_command_topic == command_topic[1]: # identify and scale raw command data passthrough_command.steering_angle = data.axes[joy_angle_axis] * joy_angle_scaler passthrough_command.speed = data.axes[joy_speed_axis] * joy_speed_scaler elif current_command_topic == command_topic[0] and listen_offboard == 'true': passthrough_command = offboard_command multiplexer_pub.publish(passthrough_command)
def finalize(self): rospy.loginfo('Halting motors, aligning wheels and exiting...') ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.speed = 0 ackermann_cmd_msg.steering_angle = 0 self.drive_pub.publish(ackermann_cmd_msg) sys.exit()
def cmd_callback(data): global wheelbase global ackermann_cmd_topic global frame_id global pub global message_type if message_type == 'ackermann_drive': v = data.linear.x steering = convert_trans_rot_vel_to_steering_angle( v, data.angular.z, wheelbase) msg = AckermannDrive() msg.steering_angle = steering msg.speed = v pub.publish(msg) else: v = data.linear.x steering = convert_trans_rot_vel_to_steering_angle( v, data.angular.z, wheelbase) msg = AckermannDriveStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.drive.steering_angle = steering msg.drive.speed = v pub.publish(msg)
def __on_camera_image(self, message): img = message.data img = np.frombuffer(img, dtype=np.uint8).reshape((message.height, message.width, 4)) img = img[380:420, :] # Segment the image by color in HSV color space img = cv2.cvtColor(img, cv2.COLOR_RGB2HSV) mask = cv2.inRange(img, np.array([50, 110, 150]), np.array([120, 255, 255])) # Find the largest segmented contour contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) command_message = AckermannDrive() command_message.speed = 50.0 command_message.steering_angle = 0.0 if contours: largest_contour = max(contours, key=cv2.contourArea) largest_contour_center = cv2.moments(largest_contour) if largest_contour_center['m00'] != 0: center_x = int(largest_contour_center['m10'] / largest_contour_center['m00']) # Find error (the lane distance from the target distance) error = center_x - 190 command_message.steering_angle = error*CONTROL_COEFFICIENT self.__ackermann_publisher.publish(command_message)
def rotate_leg(self, speed, radius, arc, update_rate): # Initialize the movement command move_cmd = AckermannDrive() # Set the forward speed # Setting no acceleration or jerk will accelerate at maximum rate move_cmd.speed = speed # Set the steering angle - Need to derive from CLMR Kinematics # TODO: Check to see that it is within the geometric limits of the robot angle = math.atan(self.wheelbase / radius) move_cmd.steering_angle = angle # Move forward for a time to go the desired distance distance = arc * radius duration = distance / speed ticks = int(duration * update_rate) for t in range(ticks): self.cmd_ack.publish(move_cmd) self.rate.sleep() # Stop the robot before the next leg self.cmd_ack.publish(AckermannDrive()) rospy.sleep(1)
def keyboard_command_callback(data): global current_command_topic global offboard_command passthrough_command = AckermannDrive() # listen to control transfer commands if rospy.get_param( '/{}/command_priority'.format(car_name)) == control_priority[0]: if not message_display[0]: rospy.loginfo(log_message.format(control_priority[0])) message_display[0] = True message_display[1] = False if current_command_topic != command_topic[0]: current_command_topic = command_topic[0] if rospy.get_param( '/{}/command_priority'.format(car_name)) == control_priority[1]: if not message_display[1]: rospy.loginfo(log_message.format(control_priority[1])) message_display[0] = False message_display[1] = True if current_command_topic != command_topic[1]: current_command_topic = command_topic[1] if current_command_topic == command_topic[1]: # identify and scale raw command data passthrough_command = data elif current_command_topic == command_topic[ 0] and listen_offboard == 'true': passthrough_command = offboard_command if rospy.get_param('/{}/suspend_control'.format(car_name)) in [ 'True', 'true', '1', '1.0' ]: passthrough_command.steering_angle = 0.0 passthrough_command.speed = 0.0 multiplexer_pub.publish(passthrough_command) else: multiplexer_pub.publish(passthrough_command)
def __init__(self): rospy.init_node("self_driving_system_bridge_node") self.steering_cmd = None self.speed_cmd = None rospy.Subscriber('/vehicle/dbw/steering_cmds/', Float32, callback=self.steering_cmd_callback) rospy.Subscriber('/vehicle/dbw/cruise_cmds', Float32, callback=self.speed_cmd_callback) ackermann_pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive) rate = rospy.Rate(30) rospy.loginfo("self-driving system bridge started") while not rospy.is_shutdown(): if self.steering_cmd is not None and self.speed_cmd is not None: ackermann_msg = AckermannDrive() ackermann_msg.steering_angle = -1.0 * float(self.steering_cmd) ackermann_msg.speed = 5 + float(self.speed_cmd) ackermann_pub.publish(ackermann_msg) rate.sleep()
def set_steer(data): car_ctl = AckermannDrive() car_msg = AckermannDriveStamped() steering_angle = rospy.Publisher('/vesc/racecar/ackermann_cmd_mux/input/teleop', AckermannDriveStamped, queue_size=1) k_distance = 6.0 k_angle = 2.0 if data.data[0] and data.data[3]: print "section 1" #error = k_distance * (data.data[4] - data.data[1]) error = k_angle * (90.0 - data.data[2]) print "k_distance", k_distance, "data.data[4]", data.data[4], "data_data[1]", data.data[1], "error", error elif data.data[0] and not data.data[3]: print "section 2" error = k_angle * (90.0 - data.data[2]) elif data.data[3] and not data.data[0]: print "section 3" error = k_angle * (-90.0 - data.data[5]) else: print "section 4" pass #car_ctl.acceleration = 0.0 car_ctl.speed = 0.8 car_ctl.steering_angle = error #steering_angle.publish('{drive: {steering_angle: ' + str(error) + ', speed: ' + str(prev_speed) + '}}') #rospy.Publisher('ackermann_cmd', AckermannDriveStamped) "publish pilot command message" car_msg.header.stamp = rospy.Time.now() car_msg.header.frame_id = "wall_follower" car_msg.drive = car_ctl print "right before publish, steering_angle: ", error steering_angle.publish(car_msg)
def control(data): global prev_error global vel_input global kp global kd global angle = 0.0 print("PID Control Node is Listening to error") ## Your PID code goes here #TODO: Use kp, ki & kd to implement a PID controller # 1. Scale the error # 2. Apply the PID equation on error to compute steering # An empty AckermannDrive message is created. You will populate the steering_angle and the speed fields. command = AckermannDrive() # TODO: Make sure the steering value is within bounds [-100,100] command.steering_angle = angle # TODO: Make sure the velocity is within bounds [0,100] command.speed = vel_input # Move the car autonomously command_pub.publish(command)
def main(): global speed speed = 64 #half the [0-127] to get 0 speed in ForwardBackward command msgAckermann = AckermannDrive() rospy.init_node('mobile_base_node', anonymous=True) rospy.Subscriber("/ackermann", AckermannDrive, callbackAckermann) rate = rospy.Rate(10) rc = Roboclaw("/dev/ttyACM1", 115200) rc.Open() address = 0x80 br = tf.TransformBroadcaster() autobot_x = 0 while not rospy.is_shutdown(): autobot_x += speed * 0.001 msgAckermann.speed = speed rc.ForwardBackwardM1(address, int(msgAckermann.speed)) #0 power forward = 64 rc.ForwardBackwardM2(address, int(-msgAckermann.speed)) #0 power backward = 64 br.sendTransform((autobot_x, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "base_link", "odom") rate.sleep()
def cmd_callback(data): global wheelbase global ackermann_cmd_topic global frame_id global pub global message_type # Hard simulation limit if data.angular.z > 1.30: data.angular.z = 1.30 elif data.angular.z < -1.30: data.angular.z = -1.30 if message_type == 'ackermann_drive': v = data.linear.x steering = data.angular.z msg = AckermannDrive() msg.steering_angle = steering msg.speed = v pub.publish(msg) else: v = data.linear.x steering = data.angular.z msg = AckermannDriveStamped() msg.header.stamp = rospy.Time.now() msg.header.frame_id = frame_id msg.drive.steering_angle = steering msg.drive.speed = v pub.publish(msg)
def call_back2(self,data): ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = data.linear.x drive.steering_angle = data.angular.z # impose limits on commanded angle # datetime object containing current date and time now = datetime.now() self.i = now.strftime("%H:%M:%S") my_name="youssf" # dd/mm/YY H:M:S image =self.overlay()[210:,:] if(self.temp != str(self.i)): with open("data_index.txt", "a") as myfile: cv2.imwrite("data/"+my_name+self.i+'.jpg',image) myfile.write("data/"+my_name+self.i+'.jpg,'+str(drive.steering_angle)+"\n") print("now =", str(self.number) + " with id = "+str(self.i)+" shape"+str(image.shape)) self.temp=str(self.i) self.number+=1 ack_cmd.drive = drive self.publisher.publish(ack_cmd)
def finalize(self): rospy.loginfo('Halting motors, aligning wheels and exiting...') self.settings = termios.tcgetattr(sys.stdin) ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.speed = 0 ackermann_cmd_msg.steering_angle = 0 self.motors_pub.publish(ackermann_cmd_msg) sys.exit()
def pub_callback(self, event): ackermann_cmd_msg = AckermannDrive() ackermann_cmd_msg.speed = self.speed ackermann_cmd_msg.steering_angle = self.steering_angle # ackermann_cmd_msg = AckermannDriveStamped() # ackermann_cmd_msg.drive.speed = self.speed # ackermann_cmd_msg.drive.steering_angle = self.steering_angle self.motors_pub.publish(ackermann_cmd_msg)
def control(self,data): car_ctl = AckermannDrive() car_msg = AckermannDriveStamped() object_angle=data.data[0]*math.pi/180 object_distance=data.data[1] if data.data[0] != -1: desired = -.3 #how wide to clear cone multiplier=1 if(not self.right): desired = .3 desired_distance=.1 distance_y = object_distance * math.cos(object_angle) if distance_y < .4: desired *= 2./3 steering_gain=.1 steering_max = .34 actual=multiplier * math.sin(object_angle)*object_distance #if actual < desired and object_distance > desired * 1.2: # actual = object_distance error=desired-actual print actual, " actual" print object_distance, " object_distance" print steering_gain * error, " steering" steering_angle = multiplier*steering_gain * error car_max_speed=1.0 #desired_distance=.1 #distance_y = object_distance * math.cos(object_angle) distance_error=distance_y-desired_distance print 'distance y ', distance_y speed_gain=.4 speed=distance_error*speed_gain print 'speed ', speed #speed=.3 car_ctl.speed = speed car_ctl.steering_angle = steering_angle car_msg.header.stamp = rospy.Time.now() car_msg.header.frame_id = "IPSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg) else: car_ctl.steering_angle = 0 car_ctl.speed = 0 #change for, later crawl forward car_msg.header.frame_id = "IPSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg)
def execute_control(self, lin_v, ang_v, time=1): while time > 0: msg = AckermannDrive() msg.speed = lin_v msg.steering_angle_velocity = ang_v self.ctrl.publish(msg) time -= 1 sleep(.2)
def stop_vehicle(self): msg = AckermannDrive() msg.speed = 0 msg.acceleration = 0 msg.jerk = 0 msg.steering_angle = 0 msg.steering_angle_velocity = 0 self.pub_ackermann_cmd.publish(msg) rospy.signal_shutdown('Made it to goal')
def set_vehicle_command(self, velocity, steering_angle): ''' Publishes the calculated steering angle ''' drive = AckermannDrive() drive.speed = velocity drive.steering_angle = steering_angle drive.steering_angle_velocity = 0.0 self.tracker_pub.publish(drive)
def twist_ackermann_node(data): command = AckermannDrive() if data.linear.x == 0.0 or data.angular.z == 0.0: command.steering_angle = 0.0 else: radius = data.linear.x / data.angular.z command.steering_angle = math.atan(wheelbase / radius) command.speed = data.linear.x command_pub.publish(command)
def drive(self, speed, angle): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = speed drive_msg.steering_angle = angle drive_msg.acceleration = 0 drive_msg.jerk = 0 drive_msg.steering_angle_velocity = 0 drive_msg_stamped.drive = drive_msg self.pub.publish(drive_msg_stamped)
def control_vehicle(speed, steering_angle): publisher = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=5) rospy.init_node('vehicle_controller', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): ackermann_cmd = AckermannDrive() ackermann_cmd.steering_angle = steering_angle ackermann_cmd.speed = speed publisher.publish(ackermann_cmd) rate.sleep()
def callback(self, data): #rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.buttons[self.enable_button]) if data.buttons[self.enable_button] is 1: ackermann_msg = AckermannDrive() if abs(data.axes[self.axis_linear]) > self.axis_dead_zone: ackermann_msg.speed = data.axes[ self.axis_linear] * self.scale_linear #left up-down stick else: ackermann_msg.speed = 0.0 if abs(data.axes[self.axis_front_steering]) > self.axis_dead_zone: ackermann_msg.steering_angle = data.axes[ self. axis_front_steering] * self.scale_front_steering #right left-right stick else: ackermann_msg.steering_angle = 0.0 self.pub.publish(ackermann_msg)
def timerCallBack(self,event): state=self.path[self.state] steer_angle = -1.*state[3] vel = self.EXT_VEL newControl = AckermannDriveStamped() newDrive = AckermannDrive() newDrive.steering_angle = steer_angle newDrive.speed = vel self.newControl.drive = newDrive self.state+=1
def callback_position(pos, pub): global last_drive_msg, current_steering_angle, current_wp, wps, finished #rospy.loginfo(rospy.get_name() + # "Got a new position: x=%f, y=%f, heading=%f\n" % # (pos.x, pos.y, pos.heading)) pos.heading = pos.heading % (2 * math.pi) drive = AckermannDrive() # If we are close enough to current waypoint, aim for next. if ap_utils.is_close_enough(wps[current_wp], pos.x, pos.y, config.WP_MARGIN): if current_wp < len(wps) - 1: rospy.loginfo("I reached waypoint %i!" % (current_wp)) current_wp += 1 elif current_wp == len(wps) - 1: rospy.loginfo("I reached my destination!\n") finished = True #elif ap_utils.is_point_behind(wps[current_wp], # pos.x, pos.y, pos.heading) and not finished: # rospy.loginfo("I missed waypoint %i!" % (current_wp)) # current_wp += 1 if not finished: drive.speed = config.SPEED else: drive.speed = 0.00 # Set speed to 0 if you are outside your "comfort zone" if not (config.MIN_X < pos.x < config.MAX_X and \ config.MIN_Y < pos.y < config.MAX_Y): drive.speed = 0.0 # Set steering angle based on current position and next waypoint current_steering_angle = ap_utils.update_steering_angle( wps[current_wp], pos.x, pos.y, pos.heading, current_steering_angle) drive.steering_angle = math.degrees(current_steering_angle) # Only publish messages if it differs from the last one if drive != last_drive_msg: last_drive_msg = drive pub.publish(drive)
def stop(self): drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = 0 drive_msg.steering_angle = 0 drive_msg.acceleration = 0 drive_msg.jerk = 0 drive_msg.steering_angle_velocity = 0 drive_msg_stamped.drive = drive_msg self.drive_pub.publish(drive_msg_stamped)
def callback(data): rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.axes[0]) ack_cmd = AckermannDriveStamped() ack_cmd.header.stamp = rospy.Time.now() drive = AckermannDrive() drive.speed = (data.axes[5]) drive.steering_angle = (data.axes[0]) ack_cmd.drive = drive ack_publisher.publish(ack_cmd)
def joy_callback(self, joy): global temp data = AckermannDrive() # x button pressed behaves as a deadman, switch R2 is forward L2 is reverse # if x is engadged and L2 and not active if joy.buttons[1] and not joy.buttons[6]: if not joy.buttons[7]: temp = 1 elif joy.buttons[7] and temp: # if rising edge on R2 drive forward # joy.axes[4] moves from 1 to -1 data.speed = self.MAX_FORWARD_VELOCITY*0.5*(-1*(joy.axes[4])+1) # also send steering angle # joy.axes[0] is left +1 to right -1 # right turn should correspond to a negative steering angle # left turn should correspond to positive steering angle data.steering_angle = joy.axes[0]*self.MAX_STEERING_ANGLE self.ackermann_publisher.publish(data) # if x is engadged and R2 and not active elif joy.buttons[1] and not joy.buttons[7]: if not joy.buttons[6]: temp = 1 elif joy.buttons[6] and temp: # if rising edge on L2 drive reverse # joy.axes[3] from 1 to -1 data.speed = self.MAX_REVERSE_VELOCITY*0.5*(joy.axes[3]-1) # also send steering angle # joy.axes[0] is left +1 to right -1 # right turn should correspond to a negative steering angle # left turn should correspond to a positivev steering angle data.steering_angle = joy.axes[0]*self.MAX_STEERING_ANGLE self.ackermann_publisher.publish(data) # deadman switch activated (i think i was trying to make a state-machine) elif (not joy.buttons[7] and temp) or not joy.buttons[1] or joy.buttons[6]: temp = 0
def apply_control(self, speed, steering_angle): self.actual_speed = speed drive_msg_stamped = AckermannDriveStamped() drive_msg = AckermannDrive() drive_msg.speed = speed drive_msg.steering_angle = steering_angle drive_msg.acceleration = 0 drive_msg.jerk = 0 drive_msg.steering_angle_velocity = 0 drive_msg_stamped.drive = drive_msg self.control_pub.publish(drive_msg_stamped)
def control(self,data): car_ctl = AckermannDrive() car_msg = AckermannDriveStamped() [centerX,centerY,width,height,actual_angle]=data.data if actual_angle != -1 and width < 180 and height < 250: desired_angle = 0 steering_gain=.4 steering_range = .34 error = desired_angle - actual_angle steering_angle = steering_gain * error car_ctl.speed = 0.3 car_ctl.steering_angle = steering_angle car_msg.header.stamp = rospy.Time.now() car_msg.header.frame_id = "IBSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg) else: car_ctl.speed = 0.0 car_msg.header.frame_id = "IBSController" car_msg.drive = car_ctl self.control_pub.publish(car_msg)