def __init__(self, mode):
self.servoCmdMsg = UInt8()
self.motorSpdCmdMsg = UInt8()
self.motorModeCmdMsg = UInt8()
self.odomMsg = Twist()
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', Twist, queue_size=1)
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 setBackgroundColor(self, r, g, b, viewport=0): '''Set the viewport's background color Parameters ---------- - r : the red component of the RGB color - g : the green component of the RGB color - b : the blue component of the RGB color - viewport : the viewport (default: all) ''' if not (isinstance( r, int ) and 0 <= r <= 255): rospy.ROSException( 'r is not a 8 bytes uint' ) if not (isinstance( g, int ) and 0 <= g <= 255): rospy.ROSException( 'g is not a 8 bytes uint' ) if not (isinstance( b, int ) and 0 <= b <= 255): rospy.ROSException( 'b is not a 8 bytes uint' ) if not isinstance(viewport, int): rospy.ROSException( 'viewport is not int' ) _r = self._to_cpp( UInt8(r) ) _g = self._to_cpp( UInt8(g) ) _b = self._to_cpp( UInt8(b) ) _viewport = self._to_cpp( Int32(viewport) ) self._viewer.setBackgroundColor( _r, _g, _b, _viewport )
def __init__(self):
rospy.init_node('dji_sdk_ros_python')
self.attitude = QuaternionStamped()
self.flight_status = UInt8()
self.battery_state = BatteryState()
self.velocity = Vector3Stamped()
self.gps_health = UInt8()
self.gps_position = NavSatFix()
self.local_position = PointStamped()
self.init_services()
self.init_subscribers()
self.init_publishers()
def control_status_publish_callback(self, time_event):
# print("34")
self.flight_status_label.configure(
text="Autonomous mode: {}".format(self.control_status))
msg = UInt8()
msg.data = self.control_status
self.control_status_msg.publish(msg)
def PID(self):
#pid konfiguartion
Kp = 180
Ki = 0.
Kd = 0.
error = (self.setpoint - abs(self.yaw))
self.aq_error = self.aq_error + error
#regulate error collection
if self.aq_error > 10:
self.aq_error = 10
elif self.aq_error < -10:
self.aq_error = -10
#reset time
current_time = rospy.Time.now()
dif_time = (current_time - self.last_time).to_sec()
#line of pid calculation kind of integral and diferential
PID = Kp * error + Ki * self.aq_error * dif_time + Kd * (
error - self.last_error) / dif_time
PID = int(round(PID))
#pid value to steering message
self.last_time = current_time
self.last_error = error
str_val = UInt8()
str_val.data = 100 + PID
if str_val.data > 180:
str_val.data = 180
elif str_val.data < 0:
str_val.data = 0
self.str_pub.publish(str_val)
def do_stop(self):
self.do_messages()
msg = UInt8()
msg.data = 1
self._mode_pub.publish(msg)
self._diags_log.publish(_err('Rover', self._rover_name,
'is stopping!'))
def envstep(self, action):
distance_before, _ = self.closest_apple()
bot_action = ord(action)
print("Envstep runnning: '%s' / [%d]" % (action, bot_action))
x = 0
th = 0
reward = 0
if action in moveBindings.keys():
x = moveBindings[action][0]
th = moveBindings[action][3]
elif action == 'p':
reward = self.try_to_pick_up_apple()
twist = Twist()
twist.linear.x = x
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = th
self.steps_to_stop = 4
self.cmd_publisher.publish(twist)
self.bot_publisher.publish(UInt8(bot_action))
observation = self.get_observation()
distance_after, _ = self.closest_apple()
# reward = reward + distance_before - distance_after
print('Closest apple was %.2f, is %.2f. Offered reward %.2f' %
(distance_before, distance_after, reward))
return (observation, reward, False, 0)
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 (self.autonomy.has_key(outcome) 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._sent_outcome_requests = []
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 __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.bridge = CvBridge()
self.go = False
self.pub_cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.traffic_light = Light('/usb_cam/image_raw/compressed')
self.sub_img = rospy.Subscriber('/usb_cam/image_raw/compressed',
CompressedImage,
self.callback,
queue_size=1)
self.sub_detect_sign = rospy.Subscriber('/detect/traffic_sign',
UInt8,
self.mode_selector,
queue_size=1)
self.mode_msg = UInt8()
def __init__(self, sentence, input_keys=[], emotion=0, block=True):
"""Constructor"""
super(SaraSay, self).__init__(outcomes=['done'], input_keys=input_keys)
if not (isinstance(sentence, types.LambdaType) and sentence.__name__
== "<lambda>") and len(input_keys) is not 0:
raise ValueError(
"In state " + type(self).__name__ + " saying \"" + sentence +
"\", you need to define a lambda function for sentence.")
# Get parameters
self.msg = say()
self.sentence = sentence
self.emotion = UInt8()
self.emotion.data = emotion
self.block = block
# Set topics
self.emotion_topic = "sara_face/Emotion"
self.say_topic = "/say"
self.sayServiceTopic = "/wm_say"
self.emotionPub = ProxyPublisher({self.emotion_topic: UInt8})
# Prepare wm_tts
if self.block:
self.sayClient = ProxyServiceCaller(
{self.sayServiceTopic: say_service})
else:
self.sayPub = ProxyPublisher({self.say_topic: say})
# Initialise the face sub
self.emotionSub = ProxySubscriberCached({self.emotion_topic: UInt8})
self.lastEmotion = None
def callback(self, data):
"""
Joy callback function
Parameters
-----------
data: :class: Joy
The joystick message to convert to twist
"""
# Camera toggle
tog = UInt8()
# Select button -> change mode
if data.buttons[6] == 1.0:
self.mode = not self.mode # TODO: set roslog
self.reset()
if self.mode:
self.loco(data)
else:
self.arm(data)
if data.buttons[4] == 1.0:
tog.data = 1
self.pub_cam.publish(tog)
elif data.buttons[5] == 1.0:
tog.data = 2
self.pub_cam.publish(tog)
def image_cb(self, msg):
"""Identifies red lights in the incoming camera image and publishes the index
of the waypoint closest to the red light's stop line to /traffic_waypoint
Args:
msg (Image): image from car-mounted camera
"""
if self.classifier_ready:
self.has_image = True
self.camera_image = msg
self.light_wp, state = self.process_traffic_lights()
'''
Publish upcoming red lights at camera frequency.
Each predicted state has to occur `STATE_COUNT_THRESHOLD` number
of times till we start using it. Otherwise the previous stable state is
used.
'''
if self.state != state:
self.state_count = 0
self.state = state
elif self.state_count >= STATE_COUNT_THRESHOLD:
self.last_state = self.state
self.state_count += 1
self.upcoming_light_pub.publish(Int32(self.light_wp))
self.upcoming_light_state_pub.publish(UInt8(self.last_state))
def test_dictionary_with_uint8(self):
from std_msgs.msg import UInt8
expected_message = UInt8(data = 0xFF)
dictionary = { 'data': expected_message.data }
message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt8', dictionary)
expected_message = serialize_deserialize(expected_message)
self.assertEqual(message, expected_message)
def test_ros_message_with_uint8(self):
from std_msgs.msg import UInt8
expected_dictionary = { 'data': 0xFF }
message = UInt8(data=expected_dictionary['data'])
message = serialize_deserialize(message)
dictionary = message_converter.convert_ros_message_to_dictionary(message)
self.assertEqual(dictionary, expected_dictionary)
def __init__(self, tello_id):
self.rate = rospy.Rate(60)
self.tello_id = tello_id
self.odometry = Odometry()
self.imu = Imu()
self.image = Image()
self.flip = UInt8()
self.empty = Empty()
self.vel = Twist()
############### Publishers ##############
self.vel_pub = rospy.Publisher("/tello/cmd_vel", Twist, queue_size=20)
self.takeoff_pub = rospy.Publisher("/tello/takeoff",
Empty,
queue_size=10)
self.land_pub = rospy.Publisher("/tello/land", Empty, queue_size=10)
self.fastmode_pub = rospy.Publisher("/tello/fast_mode",
Empty,
queue_size=10)
self.flip_pub = rospy.Publisher("/tello/flip", UInt8, queue_size=10)
########## Subscribers ##################
self.odometry_sub = rospy.Subscriber("/tello/odom", Odometry,
self.odometry_callback)
self.imu_sub = rospy.Subscriber("/tello/imu",
Imu,
self.imu_callback,
queue_size=10)
self.image_sub = rospy.Subscriber("/tello/camera/image_raw", Image,
self.image_callback)
rospy.wait_for_message("/tello/camera/image_raw", Image)
def keyboard_loop(self):
try:
while (1):
x = 0
th = 0
key = getKey()
if (key == '\x03'):
break
elif key == 'p':
self.try_to_pick_up_apple()
elif key == 'r':
self.envreset()
elif key in moveBindings.keys():
x = moveBindings[key][0]
th = moveBindings[key][3]
bot_action = botBindings[key]
print(key, bot_action)
twist = Twist()
twist.linear.x = x
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = th
print(twist)
self.steps_to_stop = 4
self.cmd_publisher.publish(twist)
self.bot_publisher.publish(UInt8(bot_action))
except Exception as e:
print(e)
def _test_mode(self, mode_id, expected_state):
rospy.Subscriber("/node_enable/node_1",
Bool,
self.callback,
callback_args="node_1")
rospy.Subscriber("/node_enable/node_2",
Bool,
self.callback,
callback_args="node_2")
rospy.Subscriber("/node_enable/node_3",
Bool,
self.callback,
callback_args="node_3")
rospy.init_node(NAME, anonymous=True)
self.node_enable_states = {}
mode_pub = rospy.Publisher("/comms/set_mode", UInt8, latch=True)
message = UInt8(data=mode_id)
mode_pub.publish(message)
timeout_t = time.time() + 10.0 * 1000 #10 seconds
while not rospy.is_shutdown() and len(
self.node_enable_states) != 3 and time.time() < timeout_t:
time.sleep(0.1)
self.assertDictEqual(expected_state, self.node_enable_states)
def bumper_callback(self, data):
msg = UInt8()
### Bumper controling linear movement ###
## Front bumpers. make forward factor = 0
if (data.data[0] or data.data[1]
or data.data[2]): #(data.data & 0b000111):
rospy.set_param("/config/forward_factor", 0.0)
rospy.set_param("/config/backward_factor", 1.0)
msg.data = 1
## Back bumpers. make backward_factor = 0
elif (data.data[3] or data.data[4] or data.data[5]):
rospy.set_param("/config/forward_factor", 1.0)
rospy.set_param("/config/backward_factor", 0.0)
msg.data = 2
## No bumper detected. can move normally
else:
rospy.set_param("/config/forward_factor", 1.0)
rospy.set_param("/config/backward_factor", 1.0)
msg.data = 0
self.pub_bumper_status.publish(msg)
### ### ###
### Bumper controling angular movement ###
if (data.data[1] or data.data[4]):
rospy.set_param("/config/spin_factor", 0.0)
else:
rospy.set_param("/config/spin_factor", 1.0)
def find_matches(self, flann, kp1, des1, image_msg):
MIN_MATCH_COUNT = 9
MIN_MSE_DECISION = 50000
self.matches = flann.knnMatch(des1, self.des, k=2)
self.good = []
for m, n in self.matches:
if m.distance < 0.7 * n.distance:
self.good.append(m)
if len(self.good) > MIN_MATCH_COUNT:
src_pts = np.float32([kp1[m.queryIdx].pt
for m in self.good]).reshape(-1, 1, 2)
dst_pts = np.float32([self.kp[m.trainIdx].pt
for m in self.good]).reshape(-1, 1, 2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
self.matchesMask = mask.ravel().tolist()
mse = self.fnCalcMSE(src_pts, dst_pts)
if mse < MIN_MSE_DECISION:
msg_sign = UInt8()
msg_sign.data = self.value
SignDetector.pub_traffic_sign.publish(msg_sign)
rospy.loginfo(self.name)
return self.value
return self.value
else:
self.matchesMask = None
return None
def steer(self):
if (self.currentSteer > 180):
self.currentSteer = 180
if (self.currentSteer < 0):
self.currentSteer = 0
# rospy.loginfo("steering: " + str(self.currentSteer))
self.__steerPub.publish(UInt8(self.currentSteer))
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 start_simulation():
pubMode = rospy.Publisher('modeSelectionTopic', UInt8, queue_size = 1)
rate = rospy.Rate(0.1)
msgMode = UInt8()
msgMode.data = SIMULATION_CODE
rate.sleep()
pubMode.publish(msgMode)
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 __init__(self, arg):
self.arg = arg
rospy.init_node('hilens', anonymous=True)
self.hilensData = UInt8()
self.dataPub = rospy.Publisher('/assignerMsg/hilensData',UInt8, queue_size=1)
self.rate = rospy.Rate(10)
rospy.on_shutdown(self._shutdown)
self.hilens_socket = socket(AF_INET,SOCK_STREAM)
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 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 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
