def __init__(self):
Camera.__init__(self)
self._sub_state = rospy.Subscriber('/mavros/state', State,
self._update_state)
self._sub_result = rospy.Subscriber('/move_base/status',
GoalStatusArray,
self._update_result)
self._pub_goal = rospy.Publisher('/move_base_simple/goal',
PoseStamped,
queue_size=10)
self._average_obstacle_range = 10
self._goal = PoseStamped()
self._state = State()
self._result = GoalStatusArray()
self._listener = tf.TransformListener()
self._seq = 0
try:
rospy.wait_for_service('/mavros/set_mode')
self._set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
except rospy.ServiceException:
rospy.loginfo('Service proxy failed')
def __init__(self):
# Set up node, pub, subs
self.status = GoalStatusArray()
rospy.init_node('keyinputpublisher')
rospy.Subscriber('crazyflie1/position_control/status',GoalStatusArray, self.status_callback)
self.inputpublisher = rospy.Publisher('keyinput',String,queue_size=2)
self.rate = rospy.Rate(500)
pygame.init()
self.win_width=320
self.win_height=240
self.screen = pygame.display.set_mode((self.win_width, self.win_height), DOUBLEBUF)
pygame.display.set_caption("Crazyflie control")
#frames = 0
#ticks = pygame.time.get_ticks()
self.screen.fill((50, 55, 60)) # background
self.titlefont = pygame.font.SysFont('hack', 20)
self.text = self.titlefont.render('Keyboard input logger for controlling crazyflie', True, (255, 255, 255)); self.screen.blit(self.text, (8,10))
self.descfont = pygame.font.SysFont('hack', 18)
self.text = self.descfont.render('Keys: w,s,a,d> move; q,e> turn; f> stop; z>land', True, (255, 255, 255)); self.screen.blit(self.text, (5, self.win_height/2 - 10))
#self.text = self.descfont.render('Press Esc to quit.', True, (255, 255, 255)); self.screen.blit(self.text, (5,self.win_height/2 + 10))
self.kinput = String()
#pygame.key.set_repeat(20)
pygame.display.flip()
rospy.spin()
def askForNextLocation(self, msg=GoalStatusArray()):
if (len(msg.status_list) != 0 and msg.status_list[-1].status == 3):
callerid = msg._connection_header['callerid']
robotName = callerid[1:-15]
self.sendDirection(robotName)
def fake_auto_demo():
rospy.init_node('autonomous_demo23')
#Requirements
pub1 = rospy.Publisher('/gps/fix', NavSatFix, queue_size=10)
pub2 = rospy.Publisher('/imu/data', Imu, queue_size=10)
pub3 = rospy.Publisher('/odometry/wheel', Odometry, queue_size=10)
pub4 = rospy.Publisher('/gps_waypoint_handler/status/gps/fix',
String,
queue_size=10)
pub5 = rospy.Publisher('/pass_gate_topic', String, queue_size=10)
#Autonomous movement
#pub5 = rospy.Publisher('/move_base/status',GoalStatusArray,queue_size=10)
#Image Detect
pub6 = rospy.Publisher('/artag_detect_topic', String, queue_size=10)
#Image Reach
pub7 = rospy.Publisher('/artag_reach_topic', String, queue_size=10)
pub8 = rospy.Publisher('/done_app_topic', String, queue_size=10)
rate = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
print("0: All sensors are good.")
print("1: All sensors except encoder are good.")
print("2: Damaged Sensors")
print("3: Got Waypoint")
print("4: Did not get any waypoint")
print("5: Reached to way point")
print("6: Did not Reached to way point")
print("7: Detected AR Tag")
print("8: Did not detect AR Tag")
print("9: Reached AR Tag")
print("10: Did not reached AR Tag")
imuMsg = Imu()
imuMsg.orientation.x = 5
imuMsg.orientation.y = 5
gpsMsg = NavSatFix()
gpsMsg.latitude = 5
gpsMsg.longitude = 5
encoderMsg = Odometry()
encoderMsg.pose.pose.position.x = 5
encoderMsg.pose.pose.position.y = 5
wpStatusMsgLow = GoalStatus()
wpStatusMsgLow.status = 3
wpStatusArray = []
wpStatusArray.append(wpStatusMsgLow)
wpStatusMsg = GoalStatusArray()
wpStatusMsg.status_list = wpStatusArray
userInput = raw_input()
if userInput == "0": #All sensors are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
pub3.publish(encoderMsg)
elif userInput == "1": # All sensors except encoder are good.
pub1.publish(gpsMsg)
pub2.publish(imuMsg)
#pub3.publish("0")
elif userInput == "2": #Damaged Sensors
pub1.publish(gpsMsg)
#pub2.publish("0")
pub3.publish(encoderMsg)
elif userInput == "3": # Got Waypoint
pub4.publish("1")
elif userInput == "4": #Did not get any waypoint
pub4.publish("0")
elif userInput == "5": #Reached to way point
#pub5.publish(wpStatusMsg)
pub4.publish("2")
#elif userInput == "6": #Did not reached to way point
#pub5.publish("0")
elif userInput == "7": #Detected Image
pub6.publish("1")
#pub5.publish("1")
elif userInput == "8": #Did not Detect Image
pub6.publish("0")
elif userInput == "9": #Reached Image
pub7.publish("1")
elif userInput == "10": #Did not reached image
pub7.publish("0")
elif userInput == "11":
pub5.publish("1")
elif userInput == "12":
pub8.publish("1")
else:
print("Invalid entry")
rate.sleep()
(0,"N"),
(45,"NE"),
(90,"E"),
(135,"SE"),
(180,"S"),
(225,"SW"),
(270,"W"),
(315,"NW"),
(360,"N")
]
########################## Globals ############################
currentState = State.Autonomous
currentZone = Zone.All
movebaseStatus = GoalStatusArray()
lastPrintMsg = ""
lastPrintDelay = 0.5
lastOpenMvMsg = tuple()
lastGoalMsg = MoveBaseActionGoal()
lastGoalId = 0
lastMoveBaseMsg = Twist()
lastJoystickMsg = Twist()
lastJoystickMsgUpdate = float(0)
lastHumanDetectionUpdate = float(0)
lastPoseMsgUpdate = float(0)
lastAutonomousGoalMsgUpdate = float(0)
lastPrintMsgUpdate = float(0)
lastEmergencyMsg = Bool()
lastCurrentPose = PoseWithCovarianceStamped()
slowDown = False
def __init__(self, tf, rospy, portable_number): #コンストラクタ
self.output_line_ = "" #アウトプット用文字列
self.tf_ = tf #クラス用tf
self.back_key_ = False
self.status_key_ = False
self.detect_mode_ = False
self.mainloop_wheel_key_ = False
self.mainloop_human_key_ = False
self.leds = [2, 7, 3] #GPIO
self.led_red_ = False
self.led_yellow_ = False
self.led_blue_ = False
self.wpi_ = wiringpi2
self.rospy_ = rospy #クラス用rospy
self.global_frame_ = 'map' + portable_number #グローバルフレーム
self.base_footprint_ = 'base_footprint' + portable_number #ベースフットプリント
self.portable_number_ = portable_number #ポータブルのナンバー
self.portable_status_ = 0 #0は待機状態
self.position_id = ["B-sen", "unkown", "elevator"]
self.pot_pos_id = self.position_id[0] #最初はB-sen #次はエレベーター
self.listener_ = tf.TransformListener() #tfのリスナー
self.odom_ = Odometry()
self.twist_ = Twist()
self.status_ = GoalStatusArray()
self.chairbot_position_ = PointStamped()
self.human_position_ = tracking_points()
self.people_position_ = People()
self.start_pos_ = PoseWithCovarianceStamped() #potスタートポジ
self.goal_pos_ = PoseStamped() #potゴールポジ
self.err_count_ = 0
self.detect_end_time = rospy.Time.from_sec(time.time()).to_sec()
self.detect_start_command = "roslaunch tms_rc_pot detect_object.launch"
# self.detect_end_command = "rosnode kill /pot"+portable_number+"/human_tracker /pot"+portable_number+"/reflec_pot"
self.detect_end_command = "rosnode kill /pot" + portable_number + "/reflec_pot"
self.rate_ = self.rospy_.Rate(10) # 10hz
self.start_pub_ = self.rospy_.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=10)
self.goal_pub_ = self.rospy_.Publisher('move_base_simple/goal',
PoseStamped,
queue_size=10)
self.vel_pub_ = self.rospy_.Publisher('mobile_base/commands/velocity',
Twist,
queue_size=10)
rospy.Subscriber('odom', Odometry, self.callback_odom,
queue_size=1) #オドメトリサブスクライバー
rospy.Subscriber('move_base/status',
GoalStatusArray,
self.callback_status,
queue_size=1) #リザルトスクライバー
rospy.Subscriber('lrf_pose',
PointStamped,
self.callback_wheelchair_position,
queue_size=1) #lrf_poseサブスクライバー
rospy.Subscriber('people',
People,
self.callback_human_position,
queue_size=1) #人サブスクライバー
rospy.Subscriber('command_msg',
String,
self.callback_command,
queue_size=10) #commandサブスクライバー
rospy.wait_for_service('move_base/clear_costmaps')
self.rect_status = rospy.ServiceProxy('move_base/clear_costmaps',
Empty)
#初期化
self.wpi_.wiringPiSetup()
self.led_initialize()
self.led_red_ = False #初期は青色ledを点灯させる
self.led_yellow_ = False
self.led_blue_ = True
def __init__(self, path_points):
self.path_points = path_points
self.goal_arr = GoalStatusArray()
self.make_goal_array()
self.position_uncertainity = 5.0