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.