def SendEmergency(self): # Send an emergency (or reset) message to the ardrone driver self.pubReset.publish(Empty())
def failsafe(self):
self.emergency_pub.publish(Empty())
self.land_pub.publish(Empty())
def callback_land(data):
global pub_land
pub_land.publish(Empty())
def SendTakeoff(self): # Send a takeoff message to the ardrone driver # Note we only send a takeoff message if the drone is landed - an unexpected takeoff is not good! if(self.status == DroneStatus.Landed): self.pubTakeoff.publish(Empty())
def main():
global xr
global yr
global vx
global xb
global zb
global xg
global yg
global ball_xs
global ball_ys
global xr_prev
global yr_prev
global goalset1
global goalset2
odom_sub = rospy.Subscriber('/odom', Odometry, odom_callBack)
ball_sub = rospy.Subscriber('/ball/position', Odometry, ball_callBack)
vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=1)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumperCallback)
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=10)
rospy.init_node('goToGoal')
rate = rospy.Rate(10)
d = 100
ez_int = 0
evx_int = 0
evx_prev = 0
count = 1
slope = 0
cept = 0
xba = 0
yba = 0
a = 1
b = 1
c = 1
timer = time()
print(timer)
while time() - timer < 0.25:
reset_odom.publish(Empty())
while not rospy.is_shutdown():
vel = Twist()
thetar = pi / 2
rospy.wait_for_message('/odom', Odometry)
if count == 1:
rospy.wait_for_message('/ball/position', Odometry)
xbi = xb
ybi = zb
starting_xr = xr
starting_yr = yr
#print('Starting point: ({},{})'.format(starting_xr,starting_yr))
#print('thetag: {}'.format(thetag))
#print('theta r: {}'.format(thetar))
#print('Goal point: ({},{})'.format(xg,yg))
if debug:
f = open("debug.dat", "w+")
while (d > 0.1) and (not rospy.is_shutdown()):
if goalset1 == False:
if count < 10:
ball_xs = np.append(ball_xs, xb)
ball_ys = np.append(ball_ys, zb)
if count >= 10:
xba = np.average(ball_xs)
yba = np.average(ball_ys)
slope = (yba - ybi) / (xba - xbi)
cept = yba - xba * slope
a = -1 * slope
c = -1 * cept
xg = (b * (b * xr - a * yr) - a * c) / (a * a + b * b)
yg = (a * (-1 * b * xr + a * yr) - b * c) / (a * a + b * b)
if xba > 0 or yba > 0:
d = np.sqrt((xg - xr)**2 + (yg - yr)**2)
goalset1 = True
ball_traj.set_ydata([-1 * slope + cept, slope + cept])
print(goalset1)
if goalset1 == True:
# Calculate angular input
temp = yr
yr = xr
xr = -temp
thetad = atan2(yg - yr, xg - xr)
thetar = atan2(yr - yr_prev, xr - xr_prev)
ez = thetad - thetar
ez = atan2(np.sin(ez), np.cos(ez))
ez_int = ez_int + ez * 0.1
az = kp_z * ez + ki_z * ez_int
xr_prev = xr
yr_prev = yr
# Calculate vel input
evx = v_d - vx
evx_int = evx_int + evx * 0.1
evx_der = (evx - evx_prev) / 0.1
vx = kp_vx * evx + ki_vx * evx_int + kd_vx * evx_der
evx_prev = evx # Update previous error in velocity
vel.linear.x = vx
vel.angular.z = az
vel_pub.publish(vel)
d = np.sqrt((xg - xr)**2 + (yg - yr)**2)
if d < 0.2:
goalset2 = True
xg = (0.8 - cept) / slope
yg = 0.8
print(
'theta = {:03.2f}\td = {:03.2f}\txr = {:03.2f}\tyr = {:03.2f}\txg = {:03.2f}\tzg = {:03.2f}\t'
.format(a, d, xr, yr, xg, yg))
if count < 10:
count += 1
marks.set_xdata(ball_xs)
marks.set_ydata(ball_ys)
ball_pos.set_xdata(xb)
ball_pos.set_ydata(zb)
robot_pos.set_xdata(xr)
robot_pos.set_ydata(yr)
goal_pos.set_xdata(xg)
goal_pos.set_ydata(yg)
plt.draw()
plt.show()
rate.sleep()
print('Starting point: ({},{})'.format(starting_xr, starting_yr))
print('Ending point: ({},{})'.format(xr, yr))
print("xg = {:0.2f}\tzg = {:0.2f}\t".format(xg, yg))
print('Reached Goal!')
plt.savefig("map.png")
break
def test_std_msgs_empty(self):
from std_msgs.msg import Empty
self.assertEquals(Empty(), Empty())
self._test_ser_deser(Empty(), Empty())
def reset_controller(self):
msg = Empty()
self.reset_controller_pub.publish(msg)
def main():
global pressed
global target_zone
global target_dist
global follow
global xr
global yr
global d
global zone1
global zone2
global zone3
sign = 1
thresh = 0.3
vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=1)
odom_sub = rospy.Subscriber('/odom', Odometry, odom_callBack)
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=10)
laser_sub = rospy.Subscriber('/scan', LaserScan, laserCallBack)
rospy.Subscriber("/mobile_base/events/bumper", BumperEvent, bumperCallback)
rospy.init_node('personFollower')
rate = rospy.Rate(10)
timer = time()
while time() - timer < 0.25:
reset_odom.publish(Empty())
while not rospy.is_shutdown():
vel = Twist()
if pressed:
break
v = 0
z = 0
if (zone1 < zone2 and zone1 < zone3 and zone1 < d_max):
target_zone = 1
target_dist = zone1
if (zone2 < zone3 and zone2 < zone1 and zone2 < d_max):
target_zone = 2
target_dist = zone2
if (zone3 < zone1 and zone3 < zone2 and zone3 < d_max):
target_zone = 3
target_dist = zone3
if (zone1 < d_max and zone2 < d_max and zone3 < d_max):
follow = False
if (np.abs(target_dist - d) / d < tolerance) and target_zone == 2:
follow = False
else:
follow = True
if follow:
if target_zone == 1:
z = 0.7
if target_zone == 3:
z = -0.7
if target_zone == 2:
z = 0
e = target_dist - d
v = kp * hysteresis(e, thresh, sign)
sign = np.sign(e)
vel.linear.x = v
vel.angular.z = z
vel_pub.publish(vel)
print("[{:0.2f}\t{:0.2f}\t{:0.2f}\ttzone:{:d}]".format(
zone1, zone2, zone3, target_zone))
def open_gripper(pub):
empty_msg = Empty()
rospy.loginfo('Opening gripper')
pub.publish(empty_msg)
def close_gripper(pub):
empty_msg = Empty()
rospy.loginfo('Closing gripper')
pub.publish(empty_msg)
if __name__ == '__main__':
rospy.init_node('h264_listener')
# TODO USE TWIST JOGGER FOR CONTROLLING DRONE MANUALLY
pub_takeoff = rospy.Publisher('/tello/takeoff',Empty,queue_size=1)
pub_land = rospy.Publisher('/tello/land',Empty,queue_size=1)
pub_vel = rospy.Publisher('/tello/cmd_vel',Twist,queue_size=1)
takeoff_msg = Empty()
cmd_vel = Twist()
try:
cmd_vel.linear.z = 0.7
pub_vel.publish(cmd_vel)
rospy.sleep(3)
cmd_vel.linear.z = 0
pub_vel.publish(cmd_vel)
main()
except BaseException:
def main():
global xr
global yr
global vx
global xr_prev
global yr_prev
i = 0
odom_sub = rospy.Subscriber('/odom', Odometry, odom_callBack)
vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=1)
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=10)
rospy.init_node('goToGoal')
rate = rospy.Rate(10)
d = 100
ez_int = 0
evx_int = 0
evx_prev = 0
timer = time()
while time() - timer < 0.25:
reset_odom.publish(Empty())
thetar = 0
while not rospy.is_shutdown():
vel = Twist()
#thetar = 0
rospy.wait_for_message('/odom', Odometry)
thetag = -1 * atan2(ygoal[i] - yr, xgoal[i] - xr) + thetar
thetag = atan2(np.sin(thetag), np.cos(thetag))
xg = xgoal[i]
yg = ygoal[i]
starting_xr = xr
starting_yr = yr
print('Starting point: ({},{})'.format(starting_xr, starting_yr))
print('thetag: {}'.format(thetag))
print('theta r: {}'.format(thetar))
print('Goal point: ({},{})'.format(xg, yg))
if debug:
f = open("debug.dat", "w+")
d = np.sqrt((xg - xr)**2 + (yg - yr)**2)
while (d > 0.2) and (not rospy.is_shutdown()):
d = np.sqrt((xg - xr)**2 + (yg - yr)**2)
# Calculate angular input
thetad = atan2(yg - yr, xg - xr)
thetar = atan2(yr - yr_prev, xr - xr_prev)
ez = thetad - thetar
ez = atan2(np.sin(ez), np.cos(ez))
ez_int = ez_int + ez * 0.1
az = kp_z * ez + ki_z * ez_int
xr_prev = xr
yr_prev = yr
# Calculate vel input
evx = v_d - vx
evx_int = evx_int + evx * 0.1
evx_der = (evx - evx_prev) / 0.1
vx = kp_vx * evx + ki_vx * evx_int + kd_vx * evx_der
evx_prev = evx # Update previous error in velocity
vel.linear.x = vx
vel.angular.z = az
vel_pub.publish(vel)
print(
"ez = {:0.2f}\tthetar = {:0.2f}\tthetag = {:0.2f}\taz = {:0.2f}\td = {:0.2f}"
.format(ez, thetar, thetag, az, d))
if debug:
f.write("pose: ({:04.2f},{:04.2f},{:03.2f})\t".format(
xr, yr, thetar))
f.write("thetad = {:03.2f}\t".format(thetad))
f.write("e = {:03.2f}, az = {:03.2f}\n\n".format(e, az))
rate.sleep()
print('Starting point: ({},{})'.format(starting_xr, starting_yr))
print('Ending point: ({},{})'.format(xr, yr))
print('Reached Goal!')
i = i + 1
if (i == 4):
break
import rospy
from std_msgs.msg import Empty
from geometry_msgs.msg import Twist
if __name__ == '__main__':
rospy.init_node('example_node', anonymous=True)
# publish commands (send to quadrotor)
pub_takeoff = rospy.Publisher('/ardrone/takeoff', Empty)
pub_land = rospy.Publisher('/ardrone/land', Empty)
pub_reset = rospy.Publisher('/ardrone/reset', Empty)
pub_twist = rospy.Publisher('/cmd_vel', Twist)
print("ready!")
rospy.sleep(1.0)
print("takeoff..")
pub_takeoff.publish(Empty())
print("turn z positive")
t = Twist()
t.angular.z = 0.3
pub_twist.publish(t)
rospy.sleep(2.0)
print("land..")
pub_land.publish(Empty())
print("done!")
def __init__(self):
# Initialize
rospy.init_node('FetchCandy', anonymous=False)
self.first = True
self.id = None
# Boolean to track whether bumper was pressed
self.bump = False
self.bump_dir = 0
# initialize camera direction
self.cam_dir = 3
self.prev_dir = 1
# How long to go forward/turn in open loop control
self.turn_dur = rospy.Duration(TURN_ANGLE / ROT_SPEED) # seconds
self.straight_dur = rospy.Duration(SIDE_LENGTH / LIN_SPEED) # seconds
# Localization determined from EKF
# Position is a tuple of the (x, y) position
self.position = None
# Variables for midpoints and home
self.home = None
self.home_id = None
self.midpoint = (-3.5, -1)
self.midpoint_home = (-0.6, -0.6)
# Boolean for candy collection
self.collected_candy = False
# Orientation is the rotation in the floor plane from initial pose
self.orientation = None
# Offset to initialize coordinates to (0,0)
self.x_off = 0
self.y_off = 0
# initialize counters
self.counter_direction = 0
# Variables to determine robot conditions
self.state = 'order'
self.blocked = False
self.see_robot = False
self.prev_state = 'order'
self.destination_reached = False
self.started_going_home = False
self.marker_reached = False
# Keep track of all the markers seen
# This is a dictionary of tag_id: distance pairs
# If the marker is lost, the value is set to None, but a marker is never removed
self.markers = {}
# create blank image for map (400x500 pixels)
#self.map = np.zeros((40*10, 50*10, 3), np.uint8)
# What to do you ctrl + c (call shutdown function written below)
rospy.on_shutdown(self.shutdown)
# Publish to topic for controlling robot
#self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
# Create a publisher which can "talk" to TurtleBot and tell it to move
self.cmd_vel = rospy.Publisher(
'wanderer_velocity_smoother/raw_cmd_vel', Twist, queue_size=10)
# Subscribe to bumper events topic
rospy.Subscriber('mobile_base/events/bumper', BumperEvent,
self.process_bump_sensing)
# Subscribe to topic for AR tags
rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.process_ar_tags)
# Subscribe to robot_pose_ekf for odometry/position information
rospy.Subscriber('/robot_pose_ekf/odom_combined',
PoseWithCovarianceStamped, self.process_ekf)
# Set up the odometry reset publisher (publishing Empty messages here will reset odom)
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=1)
# Reset odometry (these messages take about a second to get through)
timer = rospy.Time.now()
while rospy.Time.now() - timer < rospy.Duration(
1) or self.position is None:
reset_odom.publish(Empty())
# 5 HZ
self.rate = rospy.Rate(5)
self.bridge = CvBridge()
# Subscribe to camera depth topic
rospy.Subscriber('/camera/depth/image',
Image,
self.process_depth_image,
queue_size=1,
buff_size=2**24)
def bebop_position_controller_yaw_pid():
global pub_landing, copter_state_manager
global is_keyreader_finished
global current_state, time_current
global twist_current, odom_current
global speed_x, speed_y, speed_z, graph
graph_error = []
graph_current = []
graph_x = []
graph_y = []
graph_z = []
graph_error_x = []
graph_error_y = []
graph_error_z = []
graph_speed_x = []
graph_speed_y = []
graph_speed_z = []
length = 0
error_yaw = 0
Pyaw = 0.3
rate = rospy.Rate(20)
message_empty = Empty()
set_pt = []
while not rospy.is_shutdown() and not is_keyreader_finished:
time_current = rospy.get_time()
current_state = copter_state_manager.get_state(time_current)
if current_state == state_manager.COPTER_STATE_NAVIGATING:
_set_pt = way_point(length, error_yaw)
current_index = _set_pt[1]
if _set_pt[0]:
set_pt = points[current_index]
else:
set_pt = points[current_index]
current_x = odom_current.pose.pose.position.x
current_y = odom_current.pose.pose.position.y
current_z = odom_current.pose.pose.position.z
bebop_yaw = get_yaw_from_quaternion(
odom_current.pose.pose.orientation)
error_x = set_pt[0] - current_x
error_y = set_pt[1] - current_y
error_z = set_pt[2] - current_z
error_yaw_calc = math.radians(set_pt[3]) - bebop_yaw
if error_yaw_calc > math.pi:
error_yaw = error_yaw_calc - 2 * math.pi
elif error_yaw_calc < -math.pi:
error_yaw = error_yaw_calc + 2 * math.pi
else:
error_yaw = error_yaw_calc
#calculates the length of error vector (target pose - current pose)
length = math.sqrt(error_x**2 + error_y**2 + error_z**2)
speed_x = pid('x', error_x, 0.15, 0.01, 0.06, 1)
speed_y = pid('y', error_y, 0.15, 0.01, 0.06, 1)
speed_z = pid('z', error_z, 0.15, 0.01, 0.06, 1)
speed_yaw = pid('yaw', error_yaw, 0.15, 0.01, 0.06, 1)
graph_error.append([error_x, error_y, error_z, error_yaw])
graph_current.append([current_x, current_y, current_z, bebop_yaw])
graph_x.append(current_x)
graph_y.append(current_y)
graph_z.append(current_z)
graph_error_x.append(error_x)
graph_error_y.append(error_y)
graph_error_z.append(error_z)
graph_speed_x.append(speed_x)
graph_speed_y.append(speed_y)
graph_speed_z.append(speed_z)
#print "----------------------------------------"
# print "target: ", set_pt
#print "current: ", round(current_x,3), round(current_y,3), round(current_z,3), round(bebop_yaw,3)
# print "error: ", round(error_x,3), round(error_y,3), round(error_z,3), round(error_yaw,3)
#print "speed: ", round(speed_x,3), round(speed_y,3), round(speed_z,3), round(speed_yaw,3)
# print "length: ", length
# print "----"
# print "pid_x = ", pid('x',error_x,0.2,0.20.2)
# print "pid_y = ", pid('y',error_y,0.2,0.2,0.2)
# print "pid_z = ", pid('z',error_z,0.2,0.2,0.2)
#save poses error and current to txt file
# with open('poses_error.txt', 'w') as file:
# file.write(str(graph_error))
file = open('poses_error.txt', 'w')
for i in graph_error:
file.write(str(i) + "\n")
with open('poses_current.txt', 'w') as file:
file.write(str(graph_current))
with open('graph_x_list.txt', 'w') as file:
file.write(str(graph_x))
with open('graph_y_list.txt', 'w') as file:
file.write(str(graph_y))
with open('graph_z_list.txt', 'w') as file:
file.write(str(graph_z))
with open('graph_error_x_list.txt', 'w') as file:
file.write(str(graph_error_x))
with open('graph_error_y_list.txt', 'w') as file:
file.write(str(graph_error_y))
with open('graph_error_z_list.txt', 'w') as file:
file.write(str(graph_error_z))
with open('graph_speed_x_list.txt', 'w') as file:
file.write(str(graph_speed_x))
with open('graph_speed_y_list.txt', 'w') as file:
file.write(str(graph_speed_y))
with open('graph_speed_z_list.txt', 'w') as file:
file.write(str(graph_speed_z))
#if last update was within 1 second continue flying, otherwise land
if odomUpdated(time_current, 1) == True:
twist_current = Twist(Vector3(speed_x, speed_y, speed_z),
Vector3(0.0, 0.0, speed_yaw))
# twist_current = Twist(Vector3(0,0,0),Vector3(0.0, 0.0, 0.0))
else:
twist_current = Twist(Vector3(0, 0, 0), Vector3(0.0, 0.0, 0.0))
pub_landing.publish(message_empty)
print "Cannot Update Odometry >> Landing"
pub.publish(twist_current)
rate.sleep()
if __name__ == "__main__":
settings = termios.tcgetattr(sys.stdin)
vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
takeoff_pub = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=1)
landing_pub = rospy.Publisher('/ardrone/land', Empty, queue_size=1)
stop_pub = rospy.Publisher('/ardrone/reset', Empty, queue_size=1)
rospy.init_node('teleop_twist_keyboard')
try:
print msg
while (1):
order = Empty()
key = getKey()
print key
if key == 't':
takeoff_pub.publish(order)
elif key == 'l':
landing_pub.publish(order)
elif key == 'p':
stop_pub.publish(order)
elif key == 's':
twist = Twist()
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
return key
rospy.init_node('drone_teleop', anonymous=True, disable_signals=True)
bebop1_pilot = rospy.Publisher('bebop1/velocity', Twist, queue_size=10)
bebop2_pilot = rospy.Publisher('bebop2/velocity', Twist, queue_size=10)
bebop1_take_off = rospy.Publisher('bebop1/takeoff', Empty, queue_size=10)
bebop1_land = rospy.Publisher('bebop1/land', Empty, queue_size=10)
bebop2_take_off = rospy.Publisher('bebop2/takeoff', Empty, queue_size=10)
bebop2_land = rospy.Publisher('bebop2/land', Empty, queue_size=10)
bebop1_emergency = rospy.Publisher('bebop1/reset', Empty, queue_size=10)
bebop2_emergency = rospy.Publisher('bebop2/reset', Empty, queue_size=10)
take_off = Empty()
land = Empty()
emergency = Empty()
msg = 'Please Enter the following keys to move the drone\n'
twist_1 = Twist()
twist_2 = Twist()
settings = termios.tcgetattr(sys.stdin)
try:
print(msg)
while (1):
key = getKey()
if key == 'w':
twist_1.linear.x += 1
bebop1_pilot.publish(twist_1)
elif key == 'x':
twist_1.linear.x -= 1
np_arr = None
netInitFinished = False
# top camera's information.
net_init_top = True
net_top = None
meta_top = None
cam_param_top = None
det_flag_top = False
has_rev_top = False
np_arr_top = None
netInitFinished_top = False
issave = True
isshow = False
empty_msg = Empty()
start_time = time.time()
import datetime
start_t = datetime.datetime.now()
output_dir_base = os.path.join("./run_pic", "{:%Y%m%dT%H%M}".format(start_t))
if not os.path.exists(output_dir_base):
os.makedirs(output_dir_base)
output_dir_top = os.path.join(output_dir_base, 'top')
if not os.path.exists(output_dir_top):
os.makedirs(output_dir_top)
output_dir = os.path.join(output_dir_base, 'bottom')
if not os.path.exists(output_dir):
os.makedirs(output_dir)
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._pub.publish(self._topic_cancel, Empty())
scan_spot_list[start_angle] = msg.ranges[start_angle] + 10000
scan_spot_list[center_angle] = msg.ranges[center_angle] + 10000
scan_spot_list[end_angle] = msg.ranges[end_angle] + 10000
scan_spot.intensities = tuple(scan_spot_list)
scan_spot_pub.publish(scan_spot)
if __name__ == "__main__":
rospy.init_node('AutoParking')
cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
reset_pub = rospy.Publisher('/reset', Empty, queue_size=1)
msg = LaserScan()
r = rospy.Rate(10)
step = 0
twist = Twist()
reset = Empty()
while not rospy.is_shutdown():
msg = rospy.wait_for_message("/scan", LaserScan)
odom = rospy.wait_for_message("/odom", Odometry)
yaw = quaternion()
scan_done, center_angle, start_angle, end_angle = scan_parking_spot()
if step == 0:
if scan_done == True:
fining_spot, start_point, center_point, end_point = finding_spot_position(
center_angle, start_angle, end_angle)
if fining_spot == True:
theta = np.arctan2(start_point[1] - end_point[1],
start_point[0] - end_point[0])
print("=================================")
def launch():
counter = 0
def goal_callback(data):
global counter
goal_pos = data.data
# Cancel Current Move Plan
for i in range(counter):
cancel_pub.publish(GoalID(stamp=rospy.Time.from_sec(0.0), id=""))
counter = 0
print("GOAL:{}".format(goal_pos))
# Get Shortest Route
path_buf = m.shortest_path(goal_pos)
if (path_buf == None):
print("NO ROUTE")
exit()
print(path_buf)
# Write Route to csv file.
with open(output_file_path, 'w') as file:
for i, p in enumerate(path_buf):
_, x, y = m.getPos(p) # node, x, y
if i + 1 < len(path_buf) and p[0] == path_buf[i +
1][0] and i != 0:
continue
if (x == None or y == None):
print("Can't get position")
continue
counter += 1
file.write(
str(x) + ',' + str(y) + ',' + '0.0,' + '0.0,' + '0.0,' +
'1.0,' + '5.92660023892e-08' + '\n')
rospy.loginfo('poses written to ' + output_file_path)
rospy.sleep(1)
# Start Move Plan
start_journey_pub.publish(Empty())
# end of goal_callback
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
# Map Instance
print("Map Instance. MODE: Launch")
m = Map()
# init ROS node
rospy.init_node('map_router')
# Publishers
path_ready_pub = rospy.Publisher('/path_ready', Empty, queue_size=10)
path_reset_pub = rospy.Publisher('/path_reset', Empty, queue_size=10)
start_journey_pub = rospy.Publisher('/start_journey', Empty, queue_size=1)
waypoints_pub = rospy.Publisher('/waypoints',
PoseWithCovarianceStamped,
queue_size=10)
cancel_pub = rospy.Publisher('/move_base/cancel', GoalID, queue_size=1)
# Subscribers
goal_sub = rospy.Subscriber('/goal', String, goal_callback)
path_reset_pub.publish(Empty())
rospy.spin()
def callback_takeoff(data):
global pub_takeoff
pub_takeoff.publish(Empty())
def main():
counter = 0
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
# Map Instance
print("Map Instance, MODE: main")
m = Map()
# init ROS node
rospy.init_node('map_router')
# Publishers
path_ready_pub = rospy.Publisher('/path_ready', Empty, queue_size=10)
path_reset_pub = rospy.Publisher('/path_reset', Empty, queue_size=10)
start_journey_pub = rospy.Publisher('/start_journey', Empty, queue_size=1)
waypoints_pub = rospy.Publisher('/waypoints',
PoseWithCovarianceStamped,
queue_size=1)
cancel_pub = rospy.Publisher('/move_base/cancel', GoalID, queue_size=1)
# Initialize Path Queue
path_reset_pub.publish(Empty())
while (True):
# Set Goal Point
print(input_text)
goal_pos = raw_input()
# Cancel Current Move Plan
print("Plan Counter:{}".format(counter))
for i in range(counter + 1):
cancel_pub.publish(GoalID(stamp=rospy.Time.from_sec(0.0), id=""))
counter = 0
# Cancel Current Move Plan
cancel_pub.publish(GoalID(stamp=rospy.Time.from_sec(0.0), id=""))
cancel_pub.publish(GoalID(stamp=rospy.Time.from_sec(0.0), id=""))
print("GOAL:{}".format(goal_pos))
# Get Shortest Route
path_buf = m.shortest_path(goal_pos)
if (path_buf == None):
print("NO ROUTE")
exit()
print(path_buf)
# Write Route to csv file.
with open(output_file_path, 'w') as file:
for i, p in enumerate(path_buf):
_, x, y = m.getPos(p) # node, x, y
if i + 1 < len(path_buf) and p[0] == path_buf[i +
1][0] and i != 0:
continue
if (x == None or y == None):
print("Can't get position")
continue
counter += 1
file.write(
str(x) + ',' + str(y) + ',' + '0.0,' + '0.0,' + '0.0,' +
'1.0,' + '5.92660023892e-08' + '\n')
rospy.loginfo('poses written to ' + output_file_path)
rospy.sleep(1)
# Start Move Plan
start_journey_pub.publish(Empty())
def trayectoria():
global vel_pub
pub = rospy.Publisher('chatter', String, queue_size=10)
takeoff_pub = rospy.Publisher('/ardrone/takeoff', Empty, queue_size=10)
land_pub = rospy.Publisher('/ardrone/land', Empty, queue_size=10)
vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
pose_sub = rospy.Subscriber("/tf", TFMessage, pose_callback)
height_sub = rospy.Subscriber("/sonar_height", Range, height_callback)
contornos_sub = rospy.Subscriber("/Contorno", Int32, cnt_callback)
MX_sub = rospy.Subscriber("/MX", Int32, mx_callback)
MY_sub = rospy.Subscriber("/MY", Int32, my_callback)
rospy.init_node('Trayectoria', anonymous=True)
rate = rospy.Rate(10) # 10hz
opt = 2
i = 1
step = 1
while not rospy.is_shutdown():
print "X : " + "{0:.2f}".format(x_p) + " Y : " + "{0:.2f}".format(
y_p) + " Oz : " + "{0:.2f}".format(z_o) + " Z : " + str(
h_p) + " " + str(cnt)
if (opt == 1):
print "Despegando \n"
takeoff_pub.publish(Empty())
# opt = 2
elif (cnt == 0):
takeoff_pub.publish(Empty())
if (i == 1):
if (y_p < 2):
vx = controlador_proporcional(0, x_p, 1, 0.3, 0.05)
vz = controlador_proporcional(1.5, h_p, 1, 0.3, 0.05)
print "Parte 1"
enviar_velocidad(vx, 0.5, vz, 0.0)
i = 1
else:
i = 2
elif (i == 2):
if (x_p < 4):
vy = controlador_proporcional(2, y_p, 1, 0.3, 0.05)
vz = controlador_proporcional(1.5, h_p, 1, 0.3, 0.05)
enviar_velocidad(0.5, vy, vz, 0.0)
print "Parte 2"
i = 2
else:
i = 3
elif (i == 3):
if (y_p > -2):
vx = controlador_proporcional(4.0, x_p, 1, 0.3, 0.05)
vz = controlador_proporcional(1.5, h_p, 1, 0.3, 0.05)
enviar_velocidad(vx, -0.5, vz, 0.0)
print "Parte 3"
i = 2
else:
i = 4
elif (i == 4):
if (x_p > 0.20):
vy = controlador_proporcional(-2.0, y_p, 1, 0.3, 0.05)
vz = controlador_proporcional(1.5, h_p, 1, 0.3, 0.05)
print "Parte 4"
enviar_velocidad(-0.5, vy, vz, 0.0)
i = 4
else:
print "Hover \n"
enviar_velocidad(0.0, 0.0, 0.0, 0.0)
i = 1
elif (cnt == 1):
if (step == 1):
# vy = controlador_proporcional(319,mx,1,0.3,5)
# vx = controlador_proporcional(240,my,1,0.3,5)
vy = controlador_proporcional(317, mx, 0.5, 0.3, 5)
vx = controlador_proporcional(180, my, 0.5, 0.3, 5)
vz = controlador_proporcional(1, h_p, 1, 0.3, 0.05)
enviar_velocidad(vx, vy, vz, 0.0)
print "Ajustando \n"
print "MX : " + str(mx) + " MY : " + str(my)
print "vx : " + "{0:.2f}".format(
vx) + " vy : " + "{0:.2f}".format(
vy) + " vz : " + "{0:.2f}".format(vz)
ex = abs(317 - mx)
ey = abs(180 - my)
if ((ex < 5) and (ey < 5)):
step = 2
else:
step = 1
elif (step == 2):
print "Hover \n"
enviar_velocidad(0.0, 0.0, 0.0, 0.0)
print "Aterrizando \n"
land_pub.publish(Empty())
elif (opt == 1):
print "Hover \n"
enviar_velocidad(0.0, 0.0, 0.0, 0.0)
rate.sleep()
def execute(self, userdata):
global pnp, done_onions, current_state
# rospy.loginfo('Executing state: Claim')
if userdata.counter >= 50:
userdata.counter = 0
return 'timed_out'
if len(userdata.color) == 0:
return 'not_found'
max_index = len(userdata.color)
print '\nMax index is = ', max_index
pnp.onion_index = 0
for i in range(max_index):
if len(userdata.y) >= i: # The number of onions found is >= the index value I'm considering sorting now.
try:
if userdata.y[i] > -0.35 and userdata.y[i] < 0.25:
pnp.target_location_x = userdata.x[i]
pnp.target_location_y = userdata.y[i]
pnp.target_location_z = userdata.z[i]
pnp.onion_color = userdata.color[i]
pnp.onion_index = i
self.is_updated = True
break
else:
done_onions += 1
print '\nDone onions = ', done_onions
except IndexError:
pass
else:
print '\nCompleted this batch, moving on!'
if max_index == done_onions:
# print '\nAll onions are sorted!'
msg = Empty()
rospy.sleep(0.1)
self.conv_pub.publish(msg)
rospy.sleep(3.25) # With the conveyor speed controller knob at 40, takes around 3 secs to move to next batch.
self.conv_pub.publish(msg)
rospy.sleep(0.5)
userdata.x = []
userdata.y = []
userdata.z = []
userdata.color = []
userdata.counter = 0
max_index = 0
done_onions = 0
return 'move'
else:
done_onions = 0
if self.is_updated == False:
userdata.counter += 1
return 'not_updated'
else:
print '\n(x,y,z) after claim: ',pnp.target_location_x,pnp.target_location_y,pnp.target_location_z
reset_gripper()
activate_gripper()
userdata.counter = 0
current_state = vals2sid(ol=0, eefl=3, pred=2, listst=2)
rospy.sleep(0.05)
return 'updated'
def SendLand(self): # Send a landing message to the ardrone driver # Note we send this in all states, landing can do no harm self.pubLand.publish(Empty())
def main():
rospy.init_node('TurtleBot', anonymous=True)
#robot1
#-----------------------------
#This declares that your node subscribes to the /odom topic which is of type Odometry.
#When new messages are received, odometry_callback is invoked with the message as the first argument.
#rospy.Subscriber('/odom', Odometry, odometry_callback)
sub_Odom_r1 = rospy.Subscriber('/robot1/odom', Odometry, odometry_r1_callback)
#Declares that your node is publishing to the cmd/input/navi topic using the message type Twist.
#rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
pub_Vel_r1 = rospy.Publisher('/robot1/mobile_base/commands/velocity', Twist, queue_size=10)
# set up the odometry reset publisher
#rospy.Publisher('/mobile_base/commands/reset_odometry', Empty, queue_size=10)
pub_ResOdom_r1 = rospy.Publisher('/robot1/mobile_base/commands/reset_odometry', Empty, queue_size=10)
#robot2
#-----------------------------
listener_r2 = tf.TransformListener()
#This declares that your node subscribes to the /odom topic which is of type Odometry.
#When new messages are received, odometry_callback is invoked with the message as the first argument.
#rospy.Subscriber('/odom', Odometry, odometry_callback)
#sub_Odom_r2 = rospy.Subscriber('/robot2/odom', Odometry, odometry_r2_callback)
#Declares that your node is publishing to the cmd/input/navi topic using the message type Twist.
#rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
pub_Vel_r2 = rospy.Publisher('/robot2/mobile_base/commands/velocity', Twist, queue_size=10)
# set up the odometry reset publisher
#rospy.Publisher('/mobile_base/commands/reset_odometry', Empty, queue_size=10)
pub_ResOdom_r2 = rospy.Publisher('/robot2/mobile_base/commands/reset_odometry', Empty, queue_size=10)
# reset odometry (these messages take a few iterations to get through)
startingTime = time.time()
while ((time.time() - startingTime)< 0.25):
pub_ResOdom_r1.publish(Empty())
while ((time.time() - startingTime)< 0.25):
pub_ResOdom_r2.publish(Empty())
rate = rospy.Rate(10)
velR1 = Twist()
velR2 = Twist()
global current_position
global goal_position
global steer_position
global pGain
tmp_dist=euclidean_distance()
current_error=0.0
control_variable=0.0
while (tmp_dist>=0.01):
#while (True):
steer_position_r1.x=goal_position_r1.x-current_position_r1.x
steer_position_r1.y=goal_position_r1.y-current_position_r1.y
steer_position_r1.z=math.atan2(steer_position_r1.y,steer_position_r1.x)
current_error=math.degrees(steer_position_r1.z)-math.degrees(current_position_r1.z)
#Proportional part
p_error=current_error
P_term = pGain * p_error
control_variable=P_term
#Limit the angular velocity
if (control_variable>0.3):
control_variable=0.3
elif (control_variable<-0.3):
control_variable=-0.3
tmp_dist=euclidean_distance()
#Limit the linear velocity
if (tmp_dist>0.2):
tmp_dist=0.2
try:
#first follower and second robot to be followed
now = rospy.Time.now()
listener_r2.waitForTransform('/robot2/base_footprint','/robot1/base_footprint', rospy.Time(0), rospy.Duration(4))
trans,rot= listener_r2.lookupTransform('/robot2/base_footprint','/robot1/base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
print(trans)
#turtle1
#Linear velocity
velR1.linear.x = 1.5*tmp_dist
#Angular velocity
velR1.angular.z = control_variable
if (tmp_dist>=0.01):
pub_Vel_r1.publish(velR1)
#====================================
#turtle2
angular = 2* math.atan2(trans[1], trans[0])
linear = 0.2* math.sqrt(trans[0] ** 2 + trans[1] ** 2)
#Limit the angular velocity
if (angular>0.3):
angular=0.3
elif (angular<-0.3):
angular=-0.3
if (linear>0.2):
linear=0.2
velR2.linear.x=2*linear
velR2.angular.z =1.5* angular
pub_Vel_r2.publish(velR2)
rate.sleep()
rospy.loginfo("Moving into goal pose!!!")
print('Goal reached out!!!')
def land(self):
self.pilot_pub.publish(self.pre_land) #Not sure this is needed
rospy.sleep(1.0)
self.land_pub.publish(Empty())
def __trigger_update(self): # Let ROS handle the threading for me. self.update_pub.publish(Empty())
(trans,rot) = tf_listener.lookupTransform(CAMERA_FRAME, tag, t)
tagDetected = True;
rospy.loginfo('found tag: '+tags_words_mapping[tag]);
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException),e:
rospy.err(e);
tagDetected = False;
'''
tagDetected = tf_listener.frameExists(tag);
#this method should prevent previous tags from being misdetected, by clearing the buffer
if(tagDetected and not tag==prevTagDetected):
prevTagDetected = tag;
wordToPublish = tags_words_mapping[tag];
rospy.loginfo('Publishing tag: '+wordToPublish);
if(wordToPublish == 'stop'):
message = Empty();
pub_stop.publish(message);
elif(wordToPublish == 'test'):
message = Empty();
pub_test.publish(message);
else:
message = String();
message.data = wordToPublish;
pub_words.publish(message);
tf_listener = tf.TransformListener(True, rospy.Duration(0.1))
rospy.sleep(5) #wait till the tag times out (in the use context
#it's not likely to get two words within 5s)
rate.sleep()
action_server_name = '/ardrone_action_server'
client = actionlib.SimpleActionClient(action_server_name, ArdroneAction)
# Instance of import function
land = rospy.Publisher('/drone/land', Empty,
queue_size=1) #Create a Publisher to land the drone
takeoff = rospy.Publisher(
'/drone/takeoff', Empty,
queue_size=1) #Create a Publisher to takeoff the drone
move = rospy.Publisher('/cmd_vel', Twist,
queue_size=1) #Create a Publisher to move the drone
# Variable to module clases or function
move_msg = Twist() #Create the message to move the drone
land_msg = Empty() #Create the message to land the drone
takeoff_msg = Empty() #Create the message to takeoff the drone
# waits until the action server is up and running
rospy.loginfo('Waiting for action Server ' + action_server_name)
client.wait_for_server()
rospy.loginfo('Action Server Found...' + action_server_name)
# creates a goal to send to the action server
goal = ArdroneGoal()
goal.nseconds = 10 # indicates, take pictures along 10 seconds
client.send_goal(goal, feedback_cb=feedback_callback)
# You can access the SimpleAction Variable "simple_state", that will be 1 if active, and 2 when finished.
#Its a variable, better use a function like get_state.
