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
