def odometryCallback(self, msg): # Get error between waypoint and current state current_waypoint = np.array( self.waypoint_list[self.current_waypoint_index]) current_position = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ]) # orientation in quaternion form qw = msg.pose.pose.orientation.w qx = msg.pose.pose.orientation.x qy = msg.pose.pose.orientation.y qz = msg.pose.pose.orientation.z # yaw from quaternion y = np.arctan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy**2 + qz**2)) error = current_position - current_waypoint[0:3] # replace next line with rotation matrix # i_to_b = quaternion_matrix([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) i_to_b = np.eye(3) print("i_to_b needs to be fixed ---- currently set to be identity", i_to_b) error_b = i_to_b * error if np.linalg.norm(error) < self.threshold: # Get new waypoint index self.current_waypoint_index += 1 if self.cyclical_path: self.current_waypoint_index %= len(self.waypoint_list) else: if self.current_waypoint_index > len(self.waypoint_list): self.current_waypoint_index -= 1 next_waypoint = np.array( self.waypoint_list[self.current_waypoint_index]) command_msg = Command() command_msg.x = next_waypoint[0] command_msg.y = next_waypoint[1] command_msg.F = next_waypoint[2] if len(current_waypoint) <= 3: next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] delta = next_point - current_waypoint current_waypoint.append(np.atan2(delta[1], delta[0])) # altitude send directly command_msg.F = next_waypoint[2] # velocity commands, Proportional gain and saturation command_msg.x = saturate(2.0 * error_b[0], 2.0, -2.0) command_msg.y = saturate(2.0 * error_b[0], 2.0, -2.0) command_msg.z = saturate(1.507 * (current_waypoint[3] - y), 1.507, -1.507) command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE self.waypoint_pub_.publish(command_msg)
def __init__(self): # get parameters try: self.waypoint_list = rospy.get_param('~waypoints') except KeyError: rospy.logfatal('waypoints not set') rospy.signal_shutdown('Parameters not set') # how close does the MAV need to get before going to the next waypoint? self.threshold = rospy.get_param('~threshold', 5) self.cyclical_path = rospy.get_param('~cycle', True) self.prev_time = rospy.Time.now() # set up Services self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint, self.addWaypointCallback) self.remove_waypoint_service = rospy.Service('remove_waypoint', RemoveWaypoint, self.addWaypointCallback) self.set_waypoint_from_file_service = rospy.Service( 'set_waypoints_from_file', SetWaypointsFromFile, self.addWaypointCallback) # Set Up Publishers and Subscribers self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.waypoint_pub_ = rospy.Publisher('high_level_command', Command, queue_size=5, latch=True) self.current_waypoint_index = 0 command_msg = Command() current_waypoint = np.array(self.waypoint_list[0]) command_msg.x = current_waypoint[0] command_msg.y = current_waypoint[1] command_msg.F = current_waypoint[2] if len(current_waypoint) > 3: command_msg.z = current_waypoint[3] else: next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] delta = next_point - current_waypoint command_msg.z = math.atan2(delta[1], delta[0]) command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE command_msg.header = Header() command_msg.header.stamp = rospy.Time.now() self.waypoint_pub_.publish(command_msg) while not rospy.is_shutdown(): # wait for new messages and call the callback when they arrive rospy.spin()
def odometryCallback(self, msg): # Get error between waypoint and current state current_waypoint = np.array( self.waypoint_list[self.current_waypoint_index]) (r, p, y) = tf.transformations.euler_from_quaternion([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) current_position = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ]) error = current_position - current_waypoint[0:3] i_to_b = tf.transformations.quaternion_matrix([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) print "i_to_b", i_to_b error_b = i_to_b * error if np.linalg.norm(error) < self.threshold: # Get new waypoint index self.current_waypoint_index += 1 if self.cyclical_path: self.current_waypoint_index %= len(self.waypoint_list) else: if self.current_waypoint_index > len(self.waypoint_list): self.current_waypoint_index -= 1 next_waypoint = np.array( self.waypoint_list[self.current_waypoint_index]) command_msg = Command() command_msg.x = next_waypoint[0] command_msg.y = next_waypoint[1] command_msg.F = next_waypoint[2] if len(current_waypoint) <= 3: next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] delta = next_point - current_waypoint current_waypoint.append(math.atan2(delta[1], delta[0])) # altitude send directly command_msg.F = next_waypoint[2] # velocity commands, Proportional gain and saturation command_msg.x = saturate(2.0 * error_b[0], 2.0, -2.0) command_msg.y = saturate(2.0 * error_b[0], 2.0, -2.0) command_msg.z = saturate(1.507 * (current_waypoint[3] - y), 1.507, -1.507) command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE self.waypoint_pub_.publish(command_msg)
def control(self, event): command = Command() command.mode = command.MODE_PASS_THROUGH command.ignore = command.IGNORE_NONE # edit starting right here!!!! # you can get GPS sensor information: # self.currentGPS.latitude # Deg # self.currentGPS.longitude # Deg # self.currentGPS.altitude # m # self.currentGPS.speed # m/s # self.currentGPS.ground_course # atan2(east/north) # code below computes ground course in rad clockwise from the north # ground_course = self.currentGPS.ground_course # if ground_course < 0: # ground_course += 2*pi command.F = 0.75 # throttle command 0.0 to 1.0 command.x = 0.0 # aileron servo command -1.0 to 1.0 positive rolls to right command.y = -0.03 # elevator servo command -1.0 to 1.0 positive pitches up command.z = 0.0 # rudder servo command -1.0 to 1.0 positive yaws to left self.command_publisher.publish(command)
def __init__(self): # get parameters try: self.waypoint_list = rospy.get_param('~waypoints') except KeyError: rospy.logfatal('waypoints not set') rospy.signal_shutdown('Parameters not set') # how close does the MAV need to get before going to the next waypoint? self.threshold = rospy.get_param('~threshold', 5) self.cyclical_path = rospy.get_param('~cycle', True) self.prev_time = rospy.Time.now() # set up Services self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint, self.addWaypointCallback) self.remove_waypoint_service = rospy.Service('remove_waypoint', RemoveWaypoint, self.removeWaypointCallback) self.set_waypoint_from_file_service = rospy.Service('set_waypoints_from_file', SetWaypointsFromFile, self.addWaypointCallback) # Set Up Publishers and Subscribers self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.obstacle_sub_ = rospy.Subscriber('nearest_obstacle',Float32, self.obstacleCallback, queue_size=5) self.waypoint_pub_ = rospy.Publisher('high_level_command', Command, queue_size=5, latch=True) self.end_waypoint_pub_ = rospy.Publisher('last_waypoint', Int16 , queue_size = 5, latch = True) self.check_path_pub_ = rospy.Publisher('check_path',Int16, queue_size = 5, latch = True) # Initialize other variables self.current_waypoint_index = 0 self.current_yaw = 0.0 #when making a new waypoint, after clearing old waypoints, # use same yaw self.planning_flag = 1 #this flag is published and used by path planner to decide # how to plan next path. 1 = visual planning, # 2 = no visualization self.obstacle_angle = 0 #angle of current nearest obstacle detected self.obstacle_offset = 0.4 #how far to move away from obstacle after it trips threshold self.new_waypoint = rospy.ServiceProxy('/slammer/add_waypoint', AddWaypoint) command_msg = Command() current_waypoint = np.array(self.waypoint_list[0]) command_msg.x = current_waypoint[0] command_msg.y = current_waypoint[1] command_msg.F = current_waypoint[2] if len(current_waypoint) > 3: command_msg.z = current_waypoint[3] else: next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] delta = next_point - current_waypoint command_msg.z = np.atan2(delta[1], delta[0]) command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE self.waypoint_pub_.publish(command_msg) while not rospy.is_shutdown(): # wait for new messages and call the callback when they arrive rospy.spin()
def __init__(self): self.mass = rospy.get_param('dynamics/mass') self.g = rospy.get_param('dynamics/gravity',9.80665) self.thrust_max = rospy.get_param('dynamics/max_F') self.thrust_eq = (self.g*self.mass)/(self.thrust_max) self.phi = 0.0 self.theta = 0.0 self.psi = 0.0 self.p = 0.0 self.q = 0.0 self.r = 0.0 self.pnddot_c = 0.0 self.peddot_c = 0.0 self.pdddot_c = 0.0 self.r_c = 0.0 self.prev_time = rospy.get_time() self.t = 0 # Set Up Publishers and Subscribers self.cmd_sub_ = rospy.Subscriber('u_command', Command, self.urawCallback, queue_size=5) self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.command_pub_ = rospy.Publisher('v_command', Command, queue_size=5, latch=True) self.v_command = Command() self.control_mode = 0
def __init__(self): # get parameters try: self.waypoint_list = rospy.get_param('~waypoints') except KeyError: rospy.logfatal('waypoints not set') rospy.signal_shutdown('Parameters not set') # how close does the MAV need to get before going to the next waypoint? self.threshold = rospy.get_param('~threshold', 5) self.cyclical_path = rospy.get_param('~cycle', True) self.prev_time = rospy.Time.now() # set up Services self.add_waypoint_service = rospy.Service('add_waypoint', AddWaypoint, self.addWaypointCallback) self.remove_waypoint_service = rospy.Service('remove_waypoint', RemoveWaypoint, self.addWaypointCallback) self.set_waypoint_from_file_service = rospy.Service('set_waypoints_from_file', SetWaypointsFromFile, self.addWaypointCallback) # Wait a second before we publish the first waypoint while (rospy.Time.now() < rospy.Time(2.)): pass # Set Up Publishers and Subscribers self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.waypoint_pub_ = rospy.Publisher('high_level_command', Command, queue_size=5, latch=True) self.relPose_pub_ = rospy.Publisher('relative_pose', RelativePose, queue_size=5, latch=True) #Create the initial relPose estimate message relativePose_msg = RelativePose() relativePose_msg.x = 0 relativePose_msg.y = 0 relativePose_msg.z = 0 relativePose_msg.F = 0 self.relPose_pub_.publish(relativePose_msg) #Create the initial command message self.current_waypoint_index = 0 command_msg = Command() current_waypoint = np.array(self.waypoint_list[0]) command_msg.header.stamp = rospy.Time.now() command_msg.x = current_waypoint[0] command_msg.y = current_waypoint[1] command_msg.F = current_waypoint[2] if len(current_waypoint) > 3: command_msg.z = current_waypoint[3] else: command_msg.z = 0. command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE self.waypoint_pub_.publish(command_msg) while not rospy.is_shutdown(): # wait for new messages and call the callback when they arrive rospy.spin()
def send_waypoint_command(self): if self.mode_flag == 'mavros': waypoint_command_msg = PositionTarget() waypoint_command_msg.header.stamp = rospy.get_rostime() waypoint_command_msg.coordinate_frame = waypoint_command_msg.FRAME_LOCAL_NED waypoint_command_msg.type_mask = self.ned_pos_mask waypoint_command_msg.position.x = self.wp_E # E waypoint_command_msg.position.y = self.wp_N # N waypoint_command_msg.position.z = self.rendezvous_height # U # This converts an NED heading command into an ENU heading command yaw = -self.heading_command + np.radians(90.0) while yaw > np.radians(180.0): yaw = yaw - np.radians(360.0) while yaw < np.radians(-180.0): yaw = yaw + np.radians(360.0) waypoint_command_msg.yaw = yaw # Publish. self.command_pub_mavros.publish(waypoint_command_msg) else: wp_command_msg = Command() wp_command_msg.x = self.wp_N wp_command_msg.y = self.wp_E wp_command_msg.F = -self.rendezvous_height wp_command_msg.z = self.heading_command wp_command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE # Publish. self.command_pub_roscopter.publish(wp_command_msg)
def execute_landing(self): if self.mode_flag == 'mavros': # Update the status flag. self.status_flag = 'LAND' # Ramp down the motors. self.ramp_down_motors(self.landing_thrust0) # rospy.wait_for_service('/mavros/set_mode') # try: # isModeChanged = self.set_mode_srv(custom_mode='AUTO.LAND') # if isModeChanged: # self.land_mode_sent = True # print "mode %s sent." % 'AUTO.LAND' # self.status_flag = 'LAND' # except rospy.ServiceException, e: # print "service call set_mode failed: %s" % e else: command_msg = Command() command_msg.x = 0.0 command_msg.y = 0.0 command_msg.F = 0.0 command_msg.z = 0.0 command_msg.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE self.command_pub_roscopter.publish(command_msg) self.status_flag = 'LAND'
def initialize(): # CommandPubSub.status_sub = rospy.Subscriber('status', Status, CommandPubSub.status_callback) CommandPubSub.command = Command() CommandPubSub.rf_com_pub = rospy.Publisher('command', Command, queue_size=1) CommandPubSub.rc_com_pub = rospy.Publisher('high_level_command', Command, queue_size=1) CommandPubSub.timer = rospy.Timer(rospy.Duration(1.0 / 25.0), CommandPubSub.update)
def odometryCallback(self, msg): # Get error between waypoint and current state if len(self.waypoint_list)==0: current_waypoint = [np.inf,np.inf,np.inf] else: current_waypoint = np.array(self.waypoint_list[self.current_waypoint_index]) (r, p, y) = tf.transformations.euler_from_quaternion([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) current_position = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) self.pos = current_position error = np.linalg.norm(current_position - current_waypoint[0:3]) if error < self.threshold: # Get new waypoint index self.current_waypoint_index += 1 if self.cyclical_path: self.current_waypoint_index %= len(self.waypoint_list) else: # When at the last waypoint, publish the replan flag once if self.current_waypoint_index >= len(self.waypoint_list): self.current_waypoint_index -=1 if not self.last_wp_published: obstacle_flag = Int16() obstacle_flag.data = self.planning_flag self.end_waypoint_pub_.publish(obstacle_flag) self.planning_flag = 1 self.last_wp_published = True else: self.last_wp_published = False #send flag to check if path is still feasible check_path_flag = Int16() check_path_flag.data = self.current_waypoint_index self.check_path_pub_.publish(check_path_flag) next_waypoint = np.array(self.waypoint_list[self.current_waypoint_index]) command_msg = Command() command_msg.x = next_waypoint[0] command_msg.y = next_waypoint[1] command_msg.F = next_waypoint[2] if len(current_waypoint) > 3: command_msg.z = current_waypoint[3] self.current_yaw = command_msg.z else: next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] delta = next_point - current_waypoint command_msg.z = np.atan2(delta[1], delta[0]) command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE self.waypoint_pub_.publish(command_msg)
def __init__(self): # Parameters for feed forward trajectory self.alpha = 2.5 #amplitude of pn wave self.beta = 1.25 #amplitude of pe wave self.eta = -2.0 #pd self.omega_f = 1.5 #period of cycle final self.omega_s = 0.05 #period of cycle start # self.alpha = 3.0 # self.beta = 3.0 # self.eta = -1.0 # self.omega = 2#2.5 self.takeoff_time = 10#30.0 #how long before the trajectory begins(s) self.g = rospy.get_param('dynamics/gravity',9.80665) self.mass = rospy.get_param('dynamics/mass') self.theta_prev = 0.0 self.theta_dot = 0.0 self.prev_time = rospy.get_time() self.start_time = self.prev_time # Set Up Publishers and Subscribers self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.traj_pub_ = rospy.Publisher('trajectory', Command, queue_size=5, latch=True) self.uff_pub_ = rospy.Publisher('u_ff', Command, queue_size=5, latch=True) self.xdes_pub_ = rospy.Publisher('xdes_vel', Command, queue_size=5, latch=True) self.cmd = Command() self.xdes = Command() self.u_ff = Command() while not rospy.is_shutdown(): # wait for new messages and call the callback when they arrive rospy.spin()
def odometryCallback(self, msg): # Get error between waypoint and current state current_waypoint = np.array( self.waypoint_list[self.current_waypoint_index]) (r, p, y) = tf.transformations.euler_from_quaternion([ msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w ]) current_position = np.array([ msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z ]) error = np.linalg.norm(current_position - current_waypoint[0:3]) if error < self.threshold: # Get new waypoint index self.current_waypoint_index += 1 if self.cyclical_path: self.current_waypoint_index %= len(self.waypoint_list) else: if self.current_waypoint_index > len(self.waypoint_list): self.current_waypoint_index -= 1 next_waypoint = np.array( self.waypoint_list[self.current_waypoint_index]) command_msg = Command() command_msg.x = next_waypoint[0] command_msg.y = next_waypoint[1] command_msg.F = next_waypoint[2] if len(current_waypoint) > 3: command_msg.z = current_waypoint[3] else: next_point = self.waypoint_list[(self.current_waypoint_index + 1) % len(self.waypoint_list)] delta = next_point - current_waypoint command_msg.z = math.atan2(delta[1], delta[0]) command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE command_msg.header = Header() command_msg.header.stamp = rospy.Time.now() self.waypoint_pub_.publish(command_msg)
def odometryCallback(self, msg): # Get error between waypoint and current state current_waypoint = np.array(self.waypoint_list[self.current_waypoint_index]) current_position = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, -msg.pose.pose.position.z]) # orientation in quaternion form qw = msg.pose.pose.orientation.w qx = msg.pose.pose.orientation.x qy = msg.pose.pose.orientation.y qz = msg.pose.pose.orientation.z # yaw from quaternion y = np.arctan2(2*(qw*qz + qx*qy), 1 - 2*(qy**2 + qz**2)) error = np.linalg.norm(current_position - current_waypoint[0:3]) #publish the relative pose estimate relativePose_msg = RelativePose() relativePose_msg.x = current_position[0] relativePose_msg.y = current_position[1] relativePose_msg.z = y relativePose_msg.F = current_position[2] self.relPose_pub_.publish(relativePose_msg) if error < self.threshold: # Get new waypoint index self.current_waypoint_index += 1 if self.cyclical_path: self.current_waypoint_index %= len(self.waypoint_list) else: if self.current_waypoint_index > len(self.waypoint_list): self.current_waypoint_index -=1 next_waypoint = np.array(self.waypoint_list[self.current_waypoint_index]) command_msg = Command() command_msg.header.stamp = rospy.Time.now() command_msg.x = next_waypoint[0] command_msg.y = next_waypoint[1] command_msg.F = next_waypoint[2] if len(next_waypoint) > 3: command_msg.z = next_waypoint[3] else: command_msg.z = 0. command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE self.waypoint_pub_.publish(command_msg)
def __init__(self): self.mass = rospy.get_param('dynamics/mass') self.g = rospy.get_param('dynamics/gravity', 9.80665) self.thrust_max = rospy.get_param('dynamics/max_F') self.thrust_eq = (self.g * self.mass) / (self.thrust_max) self.phi = 0.0 self.theta = 0.0 self.p = 0.0 self.q = 0.0 self.r = 0.0 self.phi_c = 0.0 self.theta_c = 0.0 self.F_c = 0.0 self.r_c = 0.0 self.pd = 0 self.pd_c = 0 self.w = 0 self.control_mode = 0 self.rot_true = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.prev_time = rospy.get_time() # Set Up Publishers and Subscribers self.cmd_sub_ = rospy.Subscriber('v_command', Command, self.vcmdCallback, queue_size=5) self.hlc_sub_ = rospy.Subscriber('trajectory', Command, self.trajCallback, queue_size=5) self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.command_pub_ = rospy.Publisher('diff_flat_cmd', Command, queue_size=5, latch=True) self.df_cmd = Command()
def __init__(self): self.traj_follower = self.setup_trajectory_follower() self.att_controller = self.setup_attitude_stabilizer() self.state = state.State() self.traj = trajectory.Trajectory() self.armed = False self.t0 = 0.0 self.t_prev = 0.0 # Initialize ROS rospy.Subscriber("truth/NED", Odometry, self.stateCallback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("status", Status, self.statusCallback, queue_size=1) self.cmd_pub = rospy.Publisher("command", Command, queue_size=1) rospy.Subscriber("trajectory", Float32MultiArray, self.trajectoryCallback) self.cmd_msg = Command() self.cmd_msg.mode = Command.MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE
def resetWaypoints(self): command_msg = Command() command_msg.x = self.pos[0]-self.obstacle_offset*np.cos(self.obstacle_angle) command_msg.y = self.pos[1]-self.obstacle_offset*np.sin(self.obstacle_angle) command_msg.F = self.pos[2] command_msg.z = self.current_yaw command_msg.mode = Command.MODE_XPOS_YPOS_YAW_ALTITUDE self.waypoint_pub_.publish(command_msg) rospy.wait_for_service('/slammer/add_waypoint') try: success = self.new_waypoint(x=self.pos[0],y=self.pos[1], z=self.pos[2],yaw=self.current_yaw, radius = 0.1, difficulty = 1.0) if success: pass except rospy.ServiceException,e: print "service call add_waypoint failed: %s" %e
def __init__(self): # Init Params here self.x_vel = 2.0 self.y_vel = 2.0 self.z_vel = 0.0 # yaw rate self.alt_command = 10.0 self.delay_time = 5 # Init Publishers self.high_lvl_commands_pub = rospy.Publisher('/leo/velocity_command', Command, queue_size=10) # Init the command self.relative_cmd = Command() # begin sending commands self.sendCommands()
def __init__(self): self.pub = rospy.Publisher('/command',Command, queue_size=10, latch=True) self.command_msg = Command() self.command_msg.mode = self.command_msg.MODE_ROLL_PITCH_YAWRATE_THROTTLE self.count = True # used for initiating time self.past_start = True # used for initiating time for starting trajectory rospy.Subscriber('/odom',Odometry,self.callback) # with open('/home/matiss/demo_ws/src/demo/scripts/demo.yaml','r') as f: # param = yaml.safe_load(f) param = rospy.get_param('~') self.mass = param['dynamics']['mass'] self.g = param['dynamics']['g'] self.controller = controller(param) # initiate controller # calculate the equilibrium force self.force_adjust = param['dynamics']['mass']*param['dynamics']['g']/param['controller']['equilibrium_throttle']
def __init__(self): # Load ROS params. self.axis = rospy.get_param('~xy_axis', 'X') self.magnitude = rospy.get_param('~magnitude', 0.5) self.feed_forward = rospy.get_param('~feed_forward', 0.0) # Initialize other class variables. self.vel_x = 0.0 self.vel_y = 0.0 self.phase = 1 self.t_start = rospy.get_time() # Messages self.command_msg = Command() self.ibvs_active_msg = Bool() self.ibvs_active_msg.data = True # Initialize publishes self.command_pub = rospy.Publisher('/quadcopter/high_level_command', Command, queue_size=5, latch=True) self.ibvs_active_pub = rospy.Publisher('/quadcopter/ibvs_active', Bool, queue_size=1) self.count_rate = 0.25 # seconds self.count_timer = rospy.Timer(rospy.Duration(1.0 / self.count_rate), self.generate_velocities) # Initialize timers. self.update_rate = 20.0 self.update_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate), self.send_commands)
def __init__(self): self.velocity_controller = self.setup_velocity_controller() self.att_controller = self.setup_attitude_stabilizer() self.state = state.State() self.armed = False self.t0 = 0.0 self.t_prev = 0.0 # self.vel_des = np.array([0.0, -5.0, 0]) self.vel_des = np.array([5.0, 0.0, 0.0]) self.vel_cmd = np.array([0.0, 0.0, 0.0]) self.vel_opt = np.array([0.0, 0.0, 0.0]) # Initialize ROS rospy.Subscriber("truth/NED", Odometry, self.stateCallback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("status", Status, self.statusCallback, queue_size=1) self.cmd_pub = rospy.Publisher("command", Command, queue_size=1) rospy.Subscriber("optical_flow/velocity_cmd", Vector3, self.velocityCallback) self.cmd_msg = Command() self.cmd_msg.mode = Command.MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE
rospy.wait_for_service('/mavros/set_mode') try: isModeChanged = self.set_mode_srv(custom_mode='AUTO.LAND') if isModeChanged: self.land_mode_sent = True print "mode %s sent." % 'AUTO.LAND' self.status_flag = 'LAND' except rospy.ServiceException, e: print "service call set_mode failed: %s" % e else: command_msg = Command() command_msg.x = 0.0 command_msg.y = 0.0 command_msg.F = 0.0 command_msg.z = 0.0 command_msg.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE self.command_pub_roscopter.publish(command_msg) self.status_flag = 'LAND' def send_ibvs_command(self, flag): if self.mode_flag == 'mavros': if flag == 'inner': ibvs_command_msg = PositionTarget()
def __init__(self): self.pn = 0.0 self.pe = 0.0 self.pd = 0.0 self.u = 0.0 self.v = 0.0 self.w = 0.0 self.phi = 0.0 self.theta = 0.0 self.psi = 0.0 self.p = 0.0 self.q = 0.0 self.r = 0.0 self.pndot = 0.0 self.pedot = 0.0 self.pddot = 0.0 self.pn_d = 0.0 self.pe_d = 0.0 self.pd_d = 0.0 self.psi_d = 0.0 self.pndot_d = 0.0 self.pedot_d = 0.0 self.pddot_d = 0.0 self.pnddot_c = 0.0 self.peddot_c = 0.0 self.pdddot_c = 0.0 self.r_c = 0.0 self.control_mode = 0 self.max_roll = rospy.get_param('controller/max_roll', 1.0) self.max_yaw_rate = rospy.get_param('controller/~max_yaw_rate', 0.785) self.max_throttle = rospy.get_param('controller/max_throttle', 0.85) self.gravity = rospy.get_param('dynamics/gravity', 9.80665) self.mass = rospy.get_param('dynamics/mass', 2.865) # self.xlim = np.array([0.1,0.28,0.1,0.2,0.5,0.2,0.5]) self.xlim = np.array([0.1, 0.28, 0.1, 0.2, 1.0, 0.2, 0.5]) self.K = self.findLQRGain() self.prev_time = rospy.get_time() # Set Up Publishers and Subscribers self.xhat_sub_ = rospy.Subscriber('state', Odometry, self.odometryCallback, queue_size=5) self.cmd_sub_ = rospy.Subscriber('trajectory', Command, self.trajCallback, queue_size=5) self.uff_sub_ = rospy.Subscriber('u_ff', Command, self.uffCallback, queue_size=5) self.xdes_sub_ = rospy.Subscriber('xdes_vel', Command, self.xdesCallback, queue_size=5) self.is_flying_sub_ = rospy.Subscriber('is_flying', Bool, self.isFlyingCallback, queue_size=5) self.command_pub_ = rospy.Publisher('u_command', Command, queue_size=5, latch=True) self.u_raw = Command() self.flying = False
# add aileron control del_a = # ? # add elevator control here del_e = # ? # add rudder control here del_r # add thrust control here del_f if __name__ == '__main__': # this section communitcates with gazebo through ROS you do not need to modify # this section sub = rospy.Subscriber('/dflat', Dflat, control) pub = rospy.Publisher('/junker/command', Command, queue_size = 10) rospy.init_node('att_contr', anonymous=True) rate = rospy.Rate(150) while not rospy.is_shutdown(): cmd = Command() cmd.mode = Command.MODE_PASS_THROUGH cmd.ignore = Command.IGNORE_NONE cmd.x = del_a cmd.y = del_e cmd.z = del_r cmd.F = del_f pub.publish(cmd) rate.sleep()
def __init__(self): # set up timing variables self.hz = 100. self.rate = rospy.Rate(self.hz) self.Ts = 1/self.hz self.t = [] self.Va_c=[] self.theta_c_max = 30*np.pi/180. self.phi_max = 45*np.pi/180. self.altitude_takeoff_zone = 10 self.altitude_hold_zone = 5 self.climb_out_throttle = 1.0 self.altitude_state = 0 self.integrator_1 = 0 self.integrator_2 = 0 self.integrator_3 = 0 self.integrator_4 = 0 self.integrator_5 = 0 self.integrator_6 = 0 self.error_1 = 0 self.error_2 = 0 self.error_3 = 0 self.error_4 = 0 self.error_5 = 0 self.error_6 = 0 self.ap_differentiator_ = 0 self.at_differentiator_ = 0 self.hdot = 0 self.hdot_d = 0 self.h_d = 0 self.tau = 5 self.commands = Command() # load param file self.P = yaml.load(open('/home/nmd89/git/nathan/flight_dynamics/final_project/rosplane_ws/src/rosplane/rosplane/param/aerosonde.yaml')) # modified param values so they match the simulation self.P['AS_PITCH_KP'] = 0.0 self.P['BETA_KP'] = 0.0 self.P['BETA_KI'] = 0.0 self.P['COURSE_KI'] = 0.0 self.P['PITCH_KP'] = 1.0 self.P['PITCH_KD'] = -0.17 # trim values for control surfaces and throttle self.delta_a = self.P['TRIM_A'] self.delta_e = self.P['TRIM_E'] self.delta_r = self.P['TRIM_R'] self.delta_t = self.P['TRIM_T'] # subscribe to the MAV states and controller_commands rospy.Subscriber('/fixedwing/truth', State, self.get_states) rospy.Subscriber('/fixedwing/controller_commands', Controller_Commands, self.get_commands) # publish the commanded surface deflections self.pub = rospy.Publisher('/fixedwing/command', Command, queue_size=1) check=1 while not self.t: if check: print 'waiting for states' check=0 print 'states received' check=1 while not self.Va_c: if check: print 'waiting for commands' check=0 print 'commands received'
def __init__(self): # load ROS params # PID gains u_P = rospy.get_param('~u_P', 0.2) u_I = rospy.get_param('~u_I', 0.0) u_D = rospy.get_param('~u_D', 0.01) v_P = rospy.get_param('~v_P', 0.2) v_I = rospy.get_param('~v_I', 0.0) v_D = rospy.get_param('~v_D', 0.01) w_P = rospy.get_param('~w_P', 3.0) w_I = rospy.get_param('~w_I', 0.05) w_D = rospy.get_param('~w_D', 0.5) x_P = rospy.get_param('~x_P', 0.5) x_I = rospy.get_param('~x_I', 0.01) x_D = rospy.get_param('~x_D', 0.1) y_P = rospy.get_param('~y_P', 0.5) y_I = rospy.get_param('~y_I', 0.01) y_D = rospy.get_param('~y_D', 0.1) z_P = rospy.get_param('~z_P', 1.0) z_I = rospy.get_param('~z_I', 0.1) z_D = rospy.get_param('~z_D', 0.4) psi_P = rospy.get_param('~psi_P', 0.5) psi_I = rospy.get_param('~psi_I', 0.0) psi_D = rospy.get_param('~psi_D', 0.0) tau = rospy.get_param('~tau', 0.04) # quadcopter params self.max_thrust = rospy.get_param('dynamics/max_F', 60.0) self.mass = rospy.get_param('dynamics/mass', 3.0) self.thrust_eq = (9.80665 * self.mass) / self.max_thrust self.drag_constant = rospy.get_param('dynamics/linear_mu', 0.1) # initialize state variables self.pn = 0.0 self.pe = 0.0 self.pd = 0.0 self.phi = 0.0 self.theta = 0.0 self.psi = 0.0 self.u = 0.0 self.v = 0.0 self.w = 0.0 self.p = 0.0 self.q = 0.0 self.r = 0.0 self.ax = 0.0 self.ay = 0.0 self.az = 0.0 self.throttle = 0.0 # initialize command variables self.xc_pn = 0.0 self.xc_pe = 0.0 self.xc_pd = 0.0 self.xc_phi = 0.0 self.xc_theta = 0.0 self.xc_psi = 0.0 self.xc_u = 0.0 self.xc_v = 0.0 self.xc_pd = 0.0 self.xc_r = 0.0 self.xc_ax = 0.0 self.xc_ay = 0.0 self.xc_az = 0.0 self.xc_r = 0.0 self.xc_throttle = 0.0 # initialize saturation values self.max_roll = rospy.get_param('~max_roll', 0.15) self.max_pitch = rospy.get_param('~max_pitch', 0.15) self.max_yaw_rate = rospy.get_param('~max_yaw_rate', np.radians(45.0)) self.max_throttle = rospy.get_param('~max_throttle', 1.0) self.max_u = rospy.get_param('~max_u', 1.0) self.max_v = rospy.get_param('~max_v', 1.0) self.max_w = rospy.get_param('~max_x', 1.0) # initialize PID controllers self.PID_u = PID(u_P, u_I, u_D, None, None, tau) self.PID_v = PID(v_P, v_I, v_D, None, None, tau) self.PID_w = PID(w_P, w_I, w_D, None, None, tau) self.PID_x = PID(x_P, x_I, x_D, None, None, tau) self.PID_y = PID(y_P, y_I, y_D, None, None, tau) self.PID_z = PID(z_P, z_I, z_D, None, None, tau) self.PID_psi = PID(psi_P, psi_I, psi_D, None, None, tau) # initialize other class variables self.prev_time = 0.0 self.is_flying = False self.control_mode = 4 # MODE_XPOS_YPOS_YAW_ALTITUDE self.ibvs_active = False self.command = Command() # dynamic reconfigure self.server = Server(ControllerConfig, self.reconfigure_callback) # initialize subscribers self.state_sub = rospy.Subscriber('estimate', Odometry, self.state_callback) self.is_flying_sub = rospy.Subscriber('is_flying', Bool, self.is_flying_callback) self.cmd_sub = rospy.Subscriber('high_level_command', Command, self.cmd_callback) self.ibvs_active_sub = rospy.Subscriber('ibvs_active', Bool, self.ibvs_active_callback) # initialize publishers self.command_pub = rospy.Publisher('command', Command, queue_size=10) # initialize timer self.update_rate = 200.0 self.update_timer = rospy.Timer(rospy.Duration(1.0 / self.update_rate), self.send_command)
def __init__(self): self.length = rospy.get_param('~tether_takeoff_length', 0.2) self.takeoff_alt = rospy.get_param('~takeoff_altitude', 1.0) self.comm_alt_buf = rospy.get_param('~commanded_altitude_buffer', 0.1) self.tension_threshold = rospy.get_param('~tension_threshold', 2.0) self.tensioner_pitch = rospy.get_param('~tensioner_pitch', 0.1) # make a max value? self.kp_lateral = rospy.get_param('~kp_lateral', 0.5) self.v_max = rospy.get_param('~v_max', 1.2) self.commanded_rel_pos_buffer = rospy.get_param( '~commanded_rel_pos_buffer', 0.1) self.rel_yaw_home = pi * rospy.get_param('~rel_yaw_home', 0.0) / 180.0 self.rel_yaw_buff = pi * rospy.get_param('~commanded_rel_yaw_buffer', 10.0) / 180.0 self.sweep_angle = pi * rospy.get_param('~sweep_angle', 45.0) / 180.0 self.max_rel_yaw = pi * rospy.get_param('~max_rel_yaw', 90) / 180.0 self.sweep_rel_yaw_lower = max(-self.max_rel_yaw, self.rel_yaw_home - self.sweep_angle) + self.rel_yaw_buff self.sweep_rel_yaw_upper = min(self.max_rel_yaw, self.rel_yaw_home + self.sweep_angle) - self.rel_yaw_buff self.mcS = rospy.Service('mission_command', MissionCommand, self.MCCallback) self.tlS = rospy.Service('tether_length', TetherLength, self.TLCallback) self.tltS = rospy.ServiceProxy('tether_length_tether', TetherLength) self.uaS = rospy.Service('uav_altitude', UavAltitude, self.UACallback) self.abs_pose = None self.abs_twist = None self.rel_pose = None self.rel_yaw = 0.0 self.rel_twist = None self.Fext = None self.shipVel = rospy.get_param('~ship_vel_guess', 1.0) self.command = Command() self.command.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE self.command.ignore = 0 self.command.x = 0.0 self.command.y = 0.0 self.command.z = 0.0 self.command.F = 0.0 self.abs_ekf_sub = rospy.Subscriber("odometry", Odometry, self.abs_ekf_callback) self.rel_ekf_sub = rospy.Subscriber("rel_odometry", Odometry, self.rel_ekf_callback) self.Fext_sub = rospy.Subscriber("Fext", WrenchStamped, self.fext_callback) self.ship_v_sub = rospy.Subscriber("base/navvelned", NavVELNED, self.ship_v_callback) self.hlc_com_pub = rospy.Publisher("high_level_command", Command, queue_size=1) self.locked = False self.checkRate = rospy.Rate(2.0) self.flying = False self.inTension = False self.tensionThread = threading.Thread(target=self.establishTension) self.command_timer = rospy.Timer(rospy.Duration(1.0 / 20.0), self.update)
def send_ibvs_command(self, flag): if self.mode_flag == 'mavros': if flag == 'inner': ibvs_command_msg = PositionTarget() ibvs_command_msg.header.stamp = rospy.get_rostime() ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED ibvs_command_msg.type_mask = self.velocity_mask ibvs_command_msg.velocity.x = self.saturate( self.ibvs_x_inner, self.u_max_inner, -self.u_max_inner) + self.target_VE * np.cos( self.psi) - self.target_VN * np.sin(self.psi) ibvs_command_msg.velocity.y = self.saturate( self.ibvs_y_inner, self.v_max_inner, -self.v_max_inner) + self.target_VN * np.cos( self.psi) + self.target_VE * np.sin(self.psi) ibvs_command_msg.velocity.z = self.saturate( self.ibvs_F_inner, self.w_max_inner, -self.w_max_inner) ibvs_command_msg.yaw_rate = self.ibvs_z_inner # Publish. self.command_pub_mavros.publish(ibvs_command_msg) elif flag == 'outer': ibvs_command_msg = PositionTarget() ibvs_command_msg.header.stamp = rospy.get_rostime() ibvs_command_msg.coordinate_frame = ibvs_command_msg.FRAME_BODY_NED ibvs_command_msg.type_mask = self.velocity_mask ibvs_command_msg.velocity.x = self.saturate( self.ibvs_x, self.u_max, -self.u_max) + self.target_VE * np.cos( self.psi) - self.target_VN * np.sin(self.psi) ibvs_command_msg.velocity.y = self.saturate( self.ibvs_y, self.v_max, -self.v_max) + self.target_VN * np.cos( self.psi) + self.target_VE * np.sin(self.psi) ibvs_command_msg.velocity.z = self.saturate( self.ibvs_F, self.w_max, -self.w_max) ibvs_command_msg.yaw_rate = self.ibvs_z # Publish. self.command_pub_mavros.publish(ibvs_command_msg) else: pass else: if flag == 'inner': ibvs_command_msg = Command() ibvs_command_msg.x = self.saturate( self.ibvs_x_inner, self.u_max_inner, -self.u_max_inner) + self.target_VN * np.cos( self.psi) + self.target_VE * np.sin(self.psi) ibvs_command_msg.y = self.saturate( self.ibvs_y_inner, self.v_max_inner, -self.v_max_inner) + self.target_VE * np.cos( self.psi) - self.target_VN * np.sin(self.psi) ibvs_command_msg.F = self.saturate(self.ibvs_F_inner, self.w_max_inner, -self.w_max_inner) ibvs_command_msg.z = self.ibvs_z_inner ibvs_command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE self.command_pub_roscopter.publish(ibvs_command_msg) elif flag == 'outer': ibvs_command_msg = Command() ibvs_command_msg.x = self.saturate( self.ibvs_x, self.u_max, -self.u_max) + self.target_VN * np.cos( self.psi) + self.target_VE * np.sin(self.psi) ibvs_command_msg.y = self.saturate( self.ibvs_y, self.v_max, -self.v_max) + self.target_VE * np.cos( self.psi) - self.target_VN * np.sin(self.psi) ibvs_command_msg.F = self.saturate(self.ibvs_F, self.w_max, -self.w_max) ibvs_command_msg.z = self.ibvs_z ibvs_command_msg.mode = Command.MODE_XVEL_YVEL_YAWRATE_ALTITUDE self.command_pub_roscopter.publish(ibvs_command_msg) else: pass
#!/usr/bin/env python import math import numpy as np import rospy from pynput import keyboard from rosflight_msgs.msg import Command del_a = 0.0 del_e = 0.02967 del_r = 0.0 del_f = 0.0 cmd = Command() pub = rospy.Publisher('/fixedwing/command', Command, queue_size=10) rospy.init_node('key_contr', anonymous=True) rate = rospy.Rate(60) def on_press(key): global del_a, del_e, del_r, del_f, cmd if format(key.char) == 'w': del_e += -0.05 elif format(key.char) == 's': del_e += 0.05 elif format(key.char) == 'a': del_a += -0.05 elif format(key.char) == 'd': del_a += 0.05 elif format(key.char) == 'q': del_r += -0.05 elif format(key.char) == 'e': del_r += 0.05
rospy.init_node('benchmark_commander') rospy.loginfo('BENCHMARK COMMANDED ACTIVE') command_pub = rospy.Publisher('high_level_command', Command, queue_size=1) sleep_time = 0.05 sleep_rate = 1.0 / sleep_time rate = rospy.Rate(sleep_rate) # retrieve benchmarks bench_times = rospy.get_param('~bench_times') bench_types = rospy.get_param('~bench_types') bench_com_x = rospy.get_param('~bench_com_x') bench_com_y = rospy.get_param('~bench_com_y') bench_com_z = rospy.get_param('~bench_com_z') bench_com_F = rospy.get_param('~bench_com_F') command_k = Command() command_k.mode = Command.MODE_ROLL_PITCH_YAWRATE_THROTTLE command_k.ignore = 8 command_k.F = 0.0 command_k.x = 0.0 command_k.y = 0.0 command_k.z = 0.0 time_k = bench_times.pop(0) type_k = getCommandType(bench_types.pop(0)) comx_k = bench_com_x.pop(0) comy_k = bench_com_y.pop(0) comz_k = bench_com_z.pop(0) comF_k = bench_com_F.pop(0) idx = 0