def main():
global goal_client, exploration_client, rate, status
rospy.init_node('homework5_navigator')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler)
rospy.Subscriber("/move_base/result", MoveBaseActionResult, moveBaseActionResultHandler)
rospy.Subscriber("/explore_server/result", ExploreTaskActionResult, exploreTaskActionResultHandler)
rospy.Subscriber("/tag_detections", AprilTagDetectionArray, tagDetectionsHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
#TODO
time = 0
three_minutes = 3
# MAIN LOOP
print "exploration!"
startExploration()
def main():
global goal_client, exploration_client, rate, initial_time
rospy.init_node('fembots_finder')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped,
initialPoseHandler)
rospy.Subscriber("/tag_detections", AprilTagDetectionArray,
tagDetectionsHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
initial_time = int(round(time.time() * 1000))
#TODO
# MAIN LOOP
print "exploration!"
startExploration()
rospy.spin()
def main():
global goal_client, exploration_client, rate, initial_time
rospy.init_node('fembots_finder')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler)
rospy.Subscriber("/tag_detections", AprilTagDetectionArray, tagDetectionsHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
initial_time = int(round(time.time() * 1000))
#TODO
# MAIN LOOP
print "exploration!"
startExploration()
rospy.spin()
def main():
global point
global status
print 'what... are your ending coordinates?'
rospy.init_node('homework4_navigator')
rate = rospy.Rate(10)
sub = rospy.Subscriber('/clicked_point', PointStamped, subFn)
rospy.Subscriber("/move_base/result", MoveBaseActionResult, callback)
rospy.Subsciber("/tag_detections_pose", PoseArray, aprilFn)
while (point == None):
rate.sleep()
print "x: ", point.x
x = point.x
print "y: ", point.y
y = point.y
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
#goalPub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10)
motor = MotorState()
motor.state = 1
print "make sure you start the motors!"
#for i in xrange(20):
# motPub.publish(motor)
# rate.sleep()
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
#Waits until the action server has started up and started listening for goals
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.w = 1.0
print "Sending goal!"
client.send_goal(goal)
while (status != 3):
rate.sleep()
print "Result:", client.get_result()
def main(args):
global goal_client, exploration_client, rate
hide_time = 60 # default hide time 60 seconds
#hide_time = 30
#hide_time = 120
if (len(args) > 0):
hide_time = int(args[0])
print 'set hide time to ', hide_time
rospy.init_node('fembots_hider')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped,
initialPoseHandler)
rospy.Subscriber("/scan", LaserScan, scanHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
# MAIN LOOP
try:
t = threading.Thread(target=waitForHideTime, args=(hide_time, ))
t.start()
except:
print 'Error: couldn\'t create thread'
print "exploration!"
startExploration()
def main(args):
global goal_client, exploration_client, rate
hide_time = 60 # default hide time 60 seconds
#hide_time = 30
#hide_time = 120
if (len(args) > 0):
hide_time = int(args[0])
print 'set hide time to ' ,hide_time
rospy.init_node('fembots_hider')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler)
rospy.Subscriber("/scan", LaserScan, scanHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
# MAIN LOOP
try:
t = threading.Thread(target = waitForHideTime,args=(hide_time,))
t.start()
except:
print 'Error: couldn\'t create thread'
print "exploration!"
startExploration()
def main():
global goal_client, exploration_client, rate, status
rospy.init_node('homework5_navigator')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped,
initialPoseHandler)
rospy.Subscriber("/move_base/result", MoveBaseActionResult,
moveBaseActionResultHandler)
rospy.Subscriber("/explore_server/result", ExploreTaskActionResult,
exploreTaskActionResultHandler)
rospy.Subscriber("/tag_detections", AprilTagDetectionArray,
tagDetectionsHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
#TODO
time = 0
three_minutes = 3
# MAIN LOOP
print "exploration!"
startExploration()
def main():
global goal_client, exploration_client, rate, status
rospy.init_node('homework5_navigator')
rate = rospy.Rate(10)
rospy.Subscriber("/pose", Odometry, odometryHandler)
rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, initialPoseHandler)
rospy.Subscriber("/move_base/result", MoveBaseActionResult, moveBaseActionResultHandler)
rospy.Subscriber("/explore_server/result", ExploreTaskActionResult, exploreTaskActionResultHandler)
rospy.Subscriber("/scan", LaserScan, scanHandler)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
motor = MotorState()
motor.state = 1
print "starting motors..."
for i in xrange(20):
motPub.publish(motor)
rate.sleep()
print "\n--------------------------------------"
print "set the initial pose in rviz ya fool!!!"
print "--------------------------------------\n"
while initial_pose == None:
rate.sleep()
#TODO
# after some fixed time kill exploration server
# choose a hiding spot and navigate there
# see chooseHidingSpot
# MAIN LOOP
print "exploration!"
startExploration()
def processJoystick(self, joyMsg):
state = MotorState()
state.state = 4
self.motPub.publish(state)
joyMsg.axes = numpy.array(joyMsg.axes)
left_axis = joyMsg.axes[[0,1]]
right_axis = joyMsg.axes[[2,5]]
l2_axis = joyMsg.axes[[3]]
r2_axis = joyMsg.axes[[4]]
square, x, circle, triangle, l1, r1, l2, r2, share, options, l3, r3, ps_button, touch_button = [button == 1 for button in joyMsg.buttons]
left_dpad = joyMsg.axes[6] == 1
right_dpad = joyMsg.axes[6] == -1
up_dpad = joyMsg.axes[7] == 1
down_dpad = joyMsg.axes[7] == -1
vel = Twist()
vel.linear.x = self.max_lin_vel * left_axis[1]
if x:
vel.linear.x = vel.linear.x / 2
vel.angular.z = self.max_ang_vel * left_axis[0]
self.velPub.publish(vel)
if __name__ == "__main__":
ns = "pioneer"
sonSub = rospy.Subscriber("/sonar", SonarArray, processSonar)
odoSub = rospy.Subscriber("/pose", Odometry, processOdometry)
#publishers
velPub = rospy.Publisher("/cmd_vel", Twist)
motPub = rospy.Publisher("/cmd_motor_state", MotorState)
rate = rospy.Rate(1)
count = 0
vel = Twist()
vel.linear.x = 0.1
motor = MotorState()
motor.state = 1
while not rospy.is_shutdown():
motPub.publish(motor)
velPub.publish(vel)
rate.sleep()
if count > 15:
vel.linear.x = 0.0
velPub.publish(vel)
motor.state = 0
motPub.publish(motor)
break
count += 1
def callback(self, rgb_data, dist_data, sonar_data):
#def callback(self, rgb_data, dist_data):
# self.crt = True
# print self.crt
vel = Twist()
state = MotorState()
state.state = 4
self.motor_state_pub.publish(state)
vel.linear.x = 0.0
vel.angular.z = 0.0
if self.alo == 'right':
vel.linear.x = 0.0
vel.angular.z = -0.2
self.cmd_vel_pub.publish(vel)
rospy.sleep(4)
self.alo = ''
self.crt = False
elif self.alo == 'left':
vel.linear.x = 0.0
vel.angular.z = 0.2
self.cmd_vel_pub.publish(vel)
rospy.sleep(2)
vel.linear.x = 0.0
vel.angular.z = 0.0
self.cmd_vel_pub.publish(vel)
rospy.sleep(2)
self.alo = ''
self.crt = False
self.soundhandle.playWave(self.wavepath + "Final01.wav")
rospy.sleep(20)
self.soundhandle.playWave(self.wavepath + "Final02.wav")
rospy.sleep(22)
self.soundhandle.playWave(self.wavepath + "Final03.wav")
rospy.sleep(23)
self.soundhandle.playWave(self.wavepath + "Final04.wav")
rospy.sleep(14)
self.soundhandle.playWave(self.wavepath + "Final05.wav")
rospy.sleep(14)
self.soundhandle.playWave(self.wavepath + "Final06.wav")
rospy.sleep(20)
self.soundhandle.playWave(self.wavepath + "Final07.wav")
rospy.sleep(30)
elif self.crt:
try:
image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
except CvBridgeError, e:
print e
(rows, cols, channels) = image.shape
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
px = cols/2
py = rows/2
h_high = 179
h_low = 160
s_high = 255
s_low = 130
v_high = 255
v_low = 0
lower = np.array([h_low, s_low, v_low])
upper = np.array([h_high, s_high, v_high])
# Threshold the HSV image to get only orange colors
mask = cv2.inRange(hsv, lower, upper)
#erosion
kernel = np.ones((5,5),np.uint8)
erosion = cv2.erode(mask,kernel,iterations=3)
#dilation
dilation = cv2.dilate(erosion,kernel,iterations=3)
img = cv2.medianBlur(dilation,21)
moment = cv2.moments(img)
try:
px = int(moment['m10']/moment['m00'])
py = int(moment['m01']/moment['m00'])
area = moment['m00']
except ZeroDivisionError:
pass
cv2.circle(img, (px, py), 3, (171,110,0), 2)
cv2.circle(img, (px, py), 3, (171,110,0), 2)
#cv2.imshow('Color Segmentation', img)
sonar_array = list(sonar_data.ranges)
# if min(sonar_array[1:3]) < 0.3:
# vel.linear.x = 0.0
# vel.angular.z = -0.5
# self.fala = 1
# self.cmd_vel_pub.publish(vel)
# elif min(sonar_array[3:5]) < 0.3:
# vel.linear.x = 0.0
# vel.angular.z = 0.0
# self.fala = 1
# self.cmd_vel_pub.publish(vel)
# elif min(sonar_array[5:7]) < 0.3:
# vel.linear.x = 0.0
# vel.angular.z = 0.5
# self.fala = 1
# self.cmd_vel_pub.publish(vel)
# elif min(sonar_array[10:14]) < 0.5:
# vel.linear.x = 0.0
# vel.angular.z = 0.0
# self.fala = 1
# self.cmd_vel_pub.publish(vel)
# else:
#print "ok"
if dist_data.distance > 0.8 and dist_data.distance < 1.5:
self.motor_state_pub.publish(state)
#rospy.loginfo('lim-left: {0} < px: {1} < lim-right: {2}'.format((cols/3), px, (cols - (cols/3))))
if ((cols/3) > px and px > 0) or (dist_data.y > 0.2):# or max(sonar_array[0:3]) > 0.4:
# Goes to the left
# rospy.loginfo('left <<<<<<<<')
vel.linear.x = 0.0
vel.angular.z = 0.3
elif ((cols - (cols/3)) < px and px < cols) or (dist_data.y < -0.2):# or max(sonar_array[5:8]) > 0.4:
# Goes to the right
# rospy.loginfo('right >>>>>>>>')
vel.linear.x = 0.0
vel.angular.z = -0.3
# elif max(sonar_array[8:15]) < 0.4:
# vel.linear.x = 0.0
# vel.angular.z = 0.0
else:
# rospy.loginfo('Distancia: %.4f' % dist_data.distance)
# Foward
self.fala = 0
vel.linear.x = 0.2
#vel.linear.x = 0.25 * math.sqrt(dist_data.x**2 + dist_data.y**2)
vel.angular.z = 0.0
self.cmd_vel_pub.publish(vel)
else:
if dist_data.distance < 0.8 and self.fala == 0:
#self.crt = False
self.speech_pub.publish("I am close to you.")
self.fala = 1
elif dist_data.distance > 1.5 and self.fala == 0:
# self.speech_pub.publish("You are very fast. Please, step back and wait for me.")
self.speech_pub.publish("You are very fast. Please, wait for me.")
self.fala = 1
else:
pass
vel.linear.x = 0.0
vel.angular.z = 0.0
if __name__ == "__main__":
print 'what... are your ending coordinates?'
print "x: "
x = float(raw_input())
print "y: "
y = float(raw_input())
rospy.init_node('homework4_navigator')
rate = rospy.Rate(10)
velPub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
motPub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
#goalPub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10)
motor = MotorState()
motor.state = 1
print "make sure you start the motors!"
#for i in xrange(20):
# motPub.publish(motor)
# rate.sleep()
client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
#Waits until the action server has started up and started listening for goals
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.get_rostime()
class follow_person:
def __init__(self):
self.bridge = CvBridge()
image_sub = message_filters.Subscriber("camera/rgb/image_color", Image)
dist_sub = message_filters.Subscriber("distance_person", Distance)
self.ts = message_filters.ApproximateTimeSynchronizer([image_sub, dist_sub], 10, 0.5)
self.ts.registerCallback(self.callback)
self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.motor_state_pub = rospy.Publisher("/cmd_motor_state", MotorState, queue_size=10)
self.speech_pub = rospy.Publisher("/speech", String, queue_size=10)
self.fala = 1
def callback(self, rgb_data, dist_data):
try:
image = self.bridge.imgmsg_to_cv2(rgb_data, "bgr8")
except CvBridgeError, e:
print e
(rows, cols, channels) = image.shape
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
px = cols/2
py = rows/2
h_high = 130
h_low = 75
s_high = 255
s_low = 130
v_high = 255
v_low = 0
lower = np.array([h_low,s_low,v_low])
upper = np.array([h_high,s_high,v_high])
# Threshold the HSV image to get only orange colors
mask = cv2.inRange(hsv, lower, upper)
#erosion
kernel = np.ones((5,5),np.uint8)
erosion = cv2.erode(mask,kernel,iterations=3)
#dilation
dilation = cv2.dilate(erosion,kernel,iterations=3)
img = cv2.medianBlur(dilation,21)
moment = cv2.moments(img)
try:
px = int(moment['m10']/moment['m00'])
py = int(moment['m01']/moment['m00'])
area = moment['m00']
except ZeroDivisionError:
pass
cv2.circle(img, (px, py), 3, (171,110,0), 2)
cv2.circle(img, (px, py), 3, (171,110,0), 2)
cv2.imshow('Color Segmentation', img)
vel = Twist()
state = MotorState()
state.state = 4
if dist_data.distance > 0.7 and dist_data.distance < 1.7:
self.motor_state_pub.publish(state)
#rospy.loginfo('lim-left: {0} < px: {1} < lim-right: {2}'.format((cols/3), px, (cols - (cols/3))))
if ((cols/3) > px and px > 0) or (dist_data.y > 0.2):
#Goes to the left
#rospy.loginfo('left <<<<<<<<')
vel.linear.x = 0.0
vel.angular.z = 0.2
elif ((cols - (cols/3)) < px and px < cols) or (dist_data.y < -0.2):
#Goes to the right
#rospy.loginfo('right >>>>>>>>')
vel.linear.x = 0.0
vel.angular.z = -0.2
else:
#rospy.loginfo('Distancia: %.4f' % dist_data.distance)
#Foward
self.fala = 0
vel.linear.x = 0.2
vel.angular.z = 0.0
self.cmd_vel_pub.publish(vel)
else:
if dist_data.distance < 0.7 and self.fala == 0:
self.speech_pub.publish("I am closer to you.")
self.fala = 1
elif dist_data.distance > 1.7 and self.fala == 0:
self.speech_pub.publish("You are very fast. Please, step back and wait for me.")
self.fala = 1
else:
pass
vel.linear.x = 0.0
vel.angular.z = 0.0
self.cmd_vel_pub.publish(vel)
cv2.waitKey(3)
def enable_motors(self):
motor_state_msg = MotorState()
motor_state_msg.state = 1
self.motor_state_pub.publish(motor_state_msg)
rospy.loginfo("Motors Enabled")
self.motor_enabled = True
print sonMsg.ranges
if __name__ == "__main__":
ns = "pioneer"
sonSub = rospy.Subscriber("/sonar", SonarArray, processSonar)
odoSub = rospy.Subscriber("/pose", Odometry, processOdometry)
#publishers
velPub = rospy.Publisher("/cmd_vel", Twist)
motPub = rospy.Publisher("/cmd_motor_state", MotorState)
rate = rospy.Rate(1)
count = 0
vel = Twist()
vel.linear.x = 0.1
motor = MotorState()
motor.state = 1
while not rospy.is_shutdown():
motPub.publish(motor)
velPub.publish(vel)
rate.sleep()
if count > 15:
vel.linear.x = 0.0
velPub.publish(vel)
motor.state = 0
motPub.publish(motor)
break
count += 1
