def preset_pid_gain(pid_gain_no): # サーボモータのPIDゲインをプリセットする # プリセットの内容はcrane_x7_control/scripts/preset_reconfigure.pyに書かれている rospy.loginfo("PID Gain Preset. No." + str(pid_gain_no)) preset_no = UInt8() preset_no.data = pid_gain_no pub_preset.publish(preset_no) rospy.sleep(1) # PIDゲインがセットされるまで待つ
def close_gui(self): self.control_status = 0 msg = UInt8() for i in range(1,10): msg.data = self.control_status self.control_status_msg.publish(msg) rospy.sleep(rospy.Duration(0.05)) self.master.quit()
def __init__(self): self.mode = 0 self.left_lane = False self.right_lane = False self.center = 0 self.lastError = 0 self.MAX_VEL = 0.2 self.cam_img = np.zeros(shape=(360, 640, 3), dtype=np.uint8) self.cam_img2 = np.zeros(shape=(480, 640, 3), dtype=np.uint8) self.bridge = CvBridge() self.go = False # three-street self.right_step = 1 # Construction self.phase = 0 self.status = 0 self.check = 0 # Parking self.parking_phase = 1 self.is_left_turtlebot = False self.is_right_turtlebot = False self.found_turtlebot = False # Stop self.Chadan = 0 self.Chadan_go = False self.tunnel_step = 3 # self.pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1) self.mode_msg = UInt8() #self.line = Line('/usb_cam/image_raw/compressed') self.line = Line('/camera1/usb_cam1/image_raw/compressed') self.obstacle = Obstacle('/scan') #self.tunnel = Tunnel('/scan') self.sub_img1 = rospy.Subscriber( '/camera1/usb_cam1/image_raw/compressed', CompressedImage, self.callback1, queue_size=1) self.sub_img2 = rospy.Subscriber( '/camera2/usb_cam2/image_raw/compressed', CompressedImage, self.callback2, queue_size=1) self.sub_detect_sign = rospy.Subscriber('/detect/traffic_sign', UInt8, self.mode_selector, queue_size=1)
def flip(): print("flip") rospy.init_node('tello_node', anonymous=True) flip_pub = rospy.Publisher('/tello/flip', UInt8, queue_size=1) msg = UInt8(5) print(msg.data) msg.data = 1 flip_pub.publish(msg) sleep(3)
def deactivate(self): """Deactivate a node """ self.state = State.INACTIVE # Publish state to all other nodes msg = UInt8() msg.data = self.number self.state_publisher.publish(msg)
def ping_callback(self, msg): state = int(self.tiva.read(1)) rospy.loginfo("State: " + str(state)) # The state has changed so publish it if state != self.prev_state: msg = UInt8() msg.data = state self.pub.publish(msg) self.prev_state = state
def exposureCallback(self, msg): status_msg = UInt8() status_msg.data = config_complete self.status_pub.publish(status_msg) # Make sure auto_exposure is turned off if self.aut_exp_state == True: self._client.update_configuration({'auto_exposure': 'False'}) # Update exposure self._client.update_configuration({'exposure': msg.data})
def init_ros(self): rospy.init_node('servo_controller', anonymous=True) # Message self.servo_msg = UInt8() # Subscriber self.servo_pub = rospy.Publisher("servo", UInt8, queue_size=1, latch=True) self.servo_control() # Infinite loop
def publish_pedestrian_count(self, boxes, scores, classes): count = UInt8() count.data = 0 # We count the number of faces for i in range(boxes.shape[0]): if scores[i] > self._min_score and classes[i]==1: count.data += 1; self._pub.publish(count)
def callback(self, data): x = data.pose.pose.position.x y = data.pose.pose.position.y orientation_q = data.pose.pose.orientation orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w ] (roll, pitch, yaw) = euler_from_quaternion(orientation_list) x_index = np.int(x * self.resolution) y_index = np.int(y * self.resolution) if (x_index < 0): x_index = 0 if (x_index > ((self.map_size_x / self.resolution) - 1)): x_index = (self.map_size_x / self.resolution) - 1 if (y_index < 0): y_index = 0 if (y_index > ((self.map_size_y / self.resolution) - 1)): y_index = (self.map_size_y / self.resolution) - 1 x3, y3 = self.matrix[x_index, y_index, :] f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3 f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3 Kp = -5.0 steering = Kp * np.arctan(f_y / (f_x)) yaw = np.arctan(f_y / (f_x)) self.pub_yaw.publish(Float32(yaw)) if (f_x > 0): speed = -self.speed_value else: speed = self.speed_value if (f_y > 0): steering = -np.pi / 2 if (f_y < 0): steering = np.pi / 2 if (steering > (np.pi) / 2): steering = (np.pi) / 2 if (steering < -(np.pi) / 2): steering = -(np.pi) / 2 if (f_x > 0): speed = max(self.speed_value, speed * ((np.pi / 3) / (abs(steering) + 1))) steering = 90 + steering * (180 / np.pi) # print(steering) self.pub.publish(UInt8(steering)) if not self.shutdown_: self.pub_speed.publish(Int16(speed))
def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: # reset previously requested outcome if applicable if self._last_requested_outcome is not None and outcome == OperatableState._loopback_name: self._pub.publish( self._request_topic, OutcomeRequest(outcome=255, target=self._parent._get_path() + "/" + self.name)) self._last_requested_outcome = None # request outcome because autonomy level is too low if not self._force_transition and ( outcome in self.autonomy and self.autonomy[outcome] >= OperatableStateMachine.autonomy_level or outcome != OperatableState._loopback_name and self._get_path() in rospy.get_param( '/flexbe/breakpoints', [])): if outcome != self._last_requested_outcome: self._pub.publish( self._request_topic, OutcomeRequest( outcome=self._outcome_list.index(outcome), target=self._parent._get_path() + "/" + self.name)) rospy.loginfo("<-- Want result: %s > %s", self.name, outcome) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, False) self._last_requested_outcome = outcome outcome = OperatableState._loopback_name # autonomy level is high enough, report the executed transition elif outcome != OperatableState._loopback_name: self._last_requested_outcome = None rospy.loginfo("State result: %s > %s", self.name, outcome) self._pub.publish(self._outcome_topic, UInt8(self._outcome_list.index(outcome))) self._pub.publish( self._debug_topic, String("%s > %s" % (self._get_path(), outcome))) StateLogger.log_state_execution(self._get_path(), self.__class__.__name__, outcome, not self._force_transition, True) self._force_transition = False return outcome
def test_operatable_state(self): state = self._create() out_topic = 'flexbe/mirror/outcome' req_topic = 'flexbe/outcome_request' sub = ProxySubscriberCached({ out_topic: UInt8, req_topic: OutcomeRequest }) rospy.sleep(0.2) # wait for pub/sub # return outcome in full autonomy, no request state.result = 'error' self._execute(state) self.assertNoMessage(sub, req_topic) self.assertMessage(sub, out_topic, UInt8(1)) # request outcome on same autnomy and clear request on loopback OperatableStateMachine.autonomy_level = 2 self._execute(state) self.assertNoMessage(sub, out_topic) self.assertMessage(sub, req_topic, OutcomeRequest(outcome=1, target='/subject')) state.result = None self._execute(state) self.assertMessage(sub, req_topic, OutcomeRequest(outcome=255, target='/subject')) # still return other outcomes state.result = 'done' self._execute(state) self.assertNoMessage(sub, req_topic) self.assertMessage(sub, out_topic, UInt8(0)) # request outcome on lower autonomy, return outcome after level increase OperatableStateMachine.autonomy_level = 0 self._execute(state) self.assertNoMessage(sub, out_topic) self.assertMessage(sub, req_topic, OutcomeRequest(outcome=0, target='/subject')) OperatableStateMachine.autonomy_level = 3 self._execute(state) self.assertMessage(sub, out_topic, UInt8(0))
def joyCB(self, status, history): JoyPose6D.joyCB(self, status, history) latest = history.latest() if not latest: return if status.triangle and not latest.triangle: self.command_pub.publish(UInt8(1)) elif status.cross and not latest.cross: #reset self.resetGoalPose() elif status.circle and not latest.circle: #execute self.executePlan()
def flip(value): rate = rospy.Rate(1) pub = rospy.Publisher('/tello/flip', UInt8, queue_size=1) while pub.get_num_connections() == 0: pass msg = UInt8() msg.data = value pub.publish(msg) rate.sleep()
def update_score(self): """Update global score.""" score = self.get_score() lcd_msg = Lcd() lcd_msg.line = 1 lcd_msg.text = f"Score: {score}" self.get_logger().info(f"Score: {score}") score_msg = UInt8() score_msg.data = score self.score_publisher.publish(score_msg) self.lcd_driver.publish(lcd_msg)
def makeMsgMarker(self, numMarker, corners): #print("Make msg marker") #print(corners) new_marker = msg_marker() #new_marker = [corners, 0, 0, numMarker] for corner in corners: pixel = Point32() #print(corner) pixel.x = corner[0] pixel.y = corner[1] pixel.z = 0 #print(pixel) new_marker.Corners.append(pixel) else: new_marker.map = UInt8(0) new_marker.sector = UInt8(0) new_marker.ID = UInt8(numMarker) #print(new_marker) return new_marker print("Error make marker msg")
def __init__(self, mode): self.servoCmdMsg = UInt8() self.motorSpdCmdMsg = UInt8() self.motorModeCmdMsg = UInt8() self.odomMsg = Odometry() self.servoCmdMsg.data = 79 rospy.init_node('base_controller', anonymous=True) self.servoCmdPub = rospy.Publisher('servoCmd', UInt8, queue_size=1) self.motorSpdCmdPub = rospy.Publisher('motorSpdCmd', UInt8, queue_size=1) self.motorModeCmdPub = rospy.Publisher('motorModeCmd', UInt8, queue_size=1) self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1) self.odom_br = tf.TransformBroadcaster() self.x = 0.0 self.y = 0.0 self.theta = 0.0 self.vx = 0.0 self.vy = 0.0 self.v = 0.0 self.vth = 0.0 self.delta = 0.0 self.l = 0.58 self.cur_time = rospy.Time.now() self.last_time = rospy.Time.now() self.scale = 121.75 if mode == "PID": self.KP = 1 self.KI = 1 self.KD = 1 self.error = [0, 0, 0] rospy.Subscriber('cmd_vel', Twist, self.cmdPIDCallback) else: rospy.Subscriber('cmd_vel', Twist, self.cmdCallback) rospy.Subscriber('rpm', Int16, self.rpmCallback) self.rate = rospy.Rate(20) rospy.on_shutdown(self._shutdown)
def __init__(self): self.pub_light = rospy.Publisher("/number_pedestrians", UInt8, queue_size=5) self.pub = rospy.Publisher("/hackbike/command/target_assist_rate", UInt16, queue_size=1) self.cmd_light = UInt8() self.cmd = UInt16() self.prev_accels = np.zeros((5,3)) self.count = 0 self.prev = 200 self.max_diff = np.array([3.0, 3.0, 7.0]) self.cmd_light.data = 0 self.pub_light.publish(self.cmd_light)
def __init__(self): ## neopixel mode ## self.neopixel_mode_msg = UInt8() self.neopixel_mode_publisher = rospy.Publisher("neopixel_mode", UInt8, queue_size=10) ## neopixel rgb : [r0,g0,b0, r1,g1,b1, r2,g2,b2] ## self.neopixel_rgb_msg = UInt8MultiArray() self.neopixel_rgb_publisher = rospy.Publisher("neopixel_rgb", UInt8MultiArray, queue_size=10) rospy.init_node("last_neopixel")
def transition(self, state): """Transition the robot's state to that given. :param state: state :type state: robot.common.State """ if self.verbose: print(self.state, '->', state) self.state = state msg = UInt8() msg.data = self.state.value self.state_pub.publish(msg)
def flip(direction): start = time.time() pub = rospy.Publisher("bebop/flip", UInt8, queue_size=10) rospy.init_node('takeoff', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): pub.publish(UInt8(direction)) rate.sleep() end = time.time() if (end - start) >= 1: rospy.loginfo("Flipped") break
def main_loop(self): '''main loop get a packet from the port and parse it and publish it''' r = rospy.Rate(400) # Publish each byte as it comes in while not rospy.is_shutdown(): c = self.port.read() print c msg= UInt8() msg.data = ord(c) self.publisher.publish(msg) r.sleep()
def __init__(self, mode): self.servoCmdMsg = UInt8() self.motorSpdCmdMsg = UInt8() self.motorModeCmdMsg = UInt8() self.odomMsg = Odometry() self.servoCmdMsg.data = 90 self.wheelbase = 0.58 self.x = 0 self.y = 0 self.yaw = 0 rospy.init_node('base_controller', anonymous=True) self.last_time = rospy.Time.now() self.servoCmdPub = rospy.Publisher('servoCmd', UInt8, queue_size=1) self.motorSpdCmdPub = rospy.Publisher('motorSpdCmd', UInt8, queue_size=1) self.motorModeCmdPub = rospy.Publisher('motorModeCmd', UInt8, queue_size=1) self.odomPub = rospy.Publisher('odom', Odometry, queue_size=1) self.tfPub = tf.TransformBroadcaster() self.min_speed = 0.3 self.scale = 121.75 self.servo_PID_init(0.02, 0.03, 0.05) if mode == "PID": self.KP = 0.60 self.KI = 0.07 self.KD = 0.05 self.error = [0.0, 0.0, 0.0] rospy.Subscriber('cmd_vel', Twist, self.cmdPIDCallback) else: rospy.Subscriber('cmd_vel', Twist, self.cmdCallback) rospy.Subscriber('rpm', Int16, self.rpmCallback) self.rate = rospy.Rate(20) rospy.on_shutdown(self._shutdown)
def __init__(self): #===== Set up ROS parameters and objects =====# rospy.init_node("velocity_two_pid") self.velocity_sub = rospy.Subscriber("velocity_node/velocity", Float64, self.velocity_callback, queue_size=3) self.setpoint_sub = rospy.Subscriber("velocity_node/velocity_setpoint", Float64, self.setpoint_callback, queue_size=3) self.joystick_sub = rospy.Subscriber("/joy", Joy, self.joystick_callback) self.control_pub = rospy.Publisher("velocity_node/pedal_pwm", UInt8, queue_size=5) self.Kp1 = rospy.get_param("~Kp1", 1) # proportional gain self.Ki1 = rospy.get_param("~Ki1", 0) # integral gain self.Kd1 = rospy.get_param("~Kd1", 0) # derivative gain self.Kp2 = rospy.get_param("~Kp2", 1) # proportional gain self.Ki2 = rospy.get_param("~Ki2", 0) # integral gain self.Kd2 = rospy.get_param("~Kd2", 0) # derivative gain self.integral_mode = rospy.get_param("~integral_mode", "window") if self.integral_mode not in ["window", "continuous"]: rospy.loginfo("integral_mode value: '%s', is not a valid option. Must be either 'window' or 'continuous'. Setting to 'window'." % self.integral_mode) self.integral_mode = "window" self.deadman_button = rospy.get_param("~deadman", 4) self.deadman_on = False # this must be switched to 'True' to publish a non-zero command signal self.timeout = rospy.get_param("~timeout", 1) # number of seconds allowed since the last setpoint message before sending a 0 command self.timeout_start = time.time() self.window_size = rospy.get_param("~window_size", 30) self.delta_t = rospy.get_param("~delta_t", 1/30.) # defaults to run at 30Hz self.output_max = rospy.get_param("~output_max", 255) self.output_min = rospy.get_param("~output_min", 0) # DEBUG: print out bounding values print("[velocity_pid] Bounding control output to, Max: {0:d}, Min: {1:d}".format(self.output_max, self.output_min)) # Rate at which to run the control loop, based on 'delta_t' self.rate = rospy.Rate(1/self.delta_t) #===== Initialize variables =====# # Error terms self.e_curr = 0 self.e_prev = 0 self.e_sum = 0 self.e_sum_queue = Queue.Queue() # Control variable self.u = 0 self.u_msg = UInt8() # Process outputs self.y_setpoint = 0 self.y_curr = 0 # Times self.t_curr = time.time() self.t_prev = self.t_curr
def start_talker(): msg = UInt8() counter = 0 while not rospy.is_shutdown(): msg.data = counter % 3 pub0.publish(msg) msg.data = (counter % 3 + 1) % 3 pub1.publish(msg) msg.data = (counter % 3 + 2) % 3 pub2.publish(msg) counter += 1 rate.sleep()
def __init__(self): self.pub_light = rospy.Publisher("/number_pedestrians", UInt8, queue_size=5) self.pub = rospy.Publisher("/hackbike/command/target_assist_rate", UInt16, queue_size=1) self.cmd_light = UInt8() self.cmd = UInt16() self.min_humidity = 35.0 self.max_humidity = 55.0 self.max_assist_rate = 500 self.min_assist_rate = 100 self.prev = 200 self.cmd_light.data = 0 self.pub_light.publish(self.cmd_light)
def resetMsgs(self): bodyMsg = Twist() bodyMsg.linear.x = float(0) bodyMsg.angular.z = float(0) headMsg = JointState() headMsg.name = ["head_pan_joint", "head_tilt_joint"] headMsg.position = [float(0), float(0)] noseMsg = UInt8() noseMsg.data = int(0) self.msgs = {"body": bodyMsg, "head": headMsg, "nose": noseMsg }
def joyCB(self, status, history): JoyPose6D.joyCB(self, status, history) # broad cast tf self.br.sendTransform( (self.lleg_pose.position.x, self.lleg_pose.position.y, self.lleg_pose.position.z), (self.lleg_pose.orientation.x, self.lleg_pose.orientation.y, self.lleg_pose.orientation.z, self.lleg_pose.orientation.w), rospy.Time.now(), self.lleg_frame_id, self.frame_id) self.br.sendTransform( (self.rleg_pose.position.x, self.rleg_pose.position.y, self.rleg_pose.position.z), (self.rleg_pose.orientation.x, self.rleg_pose.orientation.y, self.rleg_pose.orientation.z, self.rleg_pose.orientation.w), rospy.Time.now(), self.rleg_frame_id, self.frame_id) latest = history.latest() if not latest: return if status.triangle and not latest.triangle: self.command_pub.publish(UInt8(1)) elif status.cross and not latest.cross: self.command_pub.publish(UInt8(2)) if status.circle and not latest.circle: base_mat = poseToMatrix(self.pre_pose.pose) lleg_offset = Pose() lleg_offset.position.y = 0.1 lleg_offset.orientation.w = 1.0 rleg_offset = Pose() rleg_offset.position.y = -0.1 rleg_offset.orientation.w = 1.0 left_offset_mat = poseToMatrix(lleg_offset) right_offset_mat = poseToMatrix(rleg_offset) new_lleg_mat = numpy.dot(base_mat, left_offset_mat) new_rleg_mat = numpy.dot(base_mat, right_offset_mat) self.lleg_pose = matrixToPose(new_lleg_mat) self.rleg_pose = matrixToPose(new_rleg_mat)
def cbTunnelStamped(self, tunnel_msg): rospy.loginfo("Tunnel Step changed from %d", self.current_step_tunnel) self.current_step_tunnel = tunnel_msg.data rospy.loginfo("into %d", self.current_step_tunnel) if self.current_step_tunnel == self.StepOfTunnel.searching_tunnel_sign.value: self.current_mode = self.CurrentMode.tunnel.value msg_mode_return = UInt8() msg_mode_return.data = self.current_mode self.pub_mode_return.publish(msg_mode_return) self.is_triggered = True
def _operatable_execute(self, *args, **kwargs): outcome = self.__execute(*args, **kwargs) if self._is_controlled: # reset previously requested outcome if applicable if self._last_requested_outcome is not None and outcome is None: self._pub.publish( self._request_topic, OutcomeRequest(outcome=255, target=self.path)) self._last_requested_outcome = None # request outcome because autonomy level is too low if not self._force_transition and ( not self.parent.is_transition_allowed(self.name, outcome) or outcome is not None and self.is_breakpoint): if outcome != self._last_requested_outcome: self._pub.publish( self._request_topic, OutcomeRequest(outcome=self.outcomes.index(outcome), target=self.path)) Logger.localinfo("<-- Want result: %s > %s" % (self.name, outcome)) StateLogger.log( 'flexbe.operator', self, type='request', request=outcome, autonomy=self.parent.autonomy_level, required=self.parent.get_required_autonomy(outcome)) self._last_requested_outcome = outcome outcome = None # autonomy level is high enough, report the executed transition elif outcome is not None and outcome in self.outcomes: Logger.localinfo("State result: %s > %s" % (self.name, outcome)) self._pub.publish(self._outcome_topic, UInt8(self.outcomes.index(outcome))) self._pub.publish(self._debug_topic, String("%s > %s" % (self.path, outcome))) if self._force_transition: StateLogger.log('flexbe.operator', self, type='forced', forced=outcome, requested=self._last_requested_outcome) self._last_requested_outcome = None self._force_transition = False return outcome