def set_vel_body_yaw_rate(self, vel_body, yaw_rate, freq): """ Offboard method that sends velocity_body and yaw_rate references to the PX4 autopilot of the the drone. Makes convertions from the body NED frame adopted for the ISR Flying Arena to the body ENU frame used by ROS. Parameters ---------- vel_body : np.array of floats with shape (3,1) Desired linear velocity for the drone, in body NED coordinates. yaw_rate : float Desired yaw rate for the vehicle, in radians per second. freq : float Topic publishing frequency, in Hz. """ pub = rospy.Publisher(self.info.drone_ns + '/mavros/setpoint_raw/local', PositionTarget, queue_size=1) msg = PositionTarget() msg.header = Header() msg.header.stamp = rospy.Time.now() msg.coordinate_frame = 8 msg.type_mask = 1991 msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel_body) msg.yaw_rate = -yaw_rate pub.publish(msg) rate = rospy.Rate(freq) rate.sleep()
def main(): rospy.init_node('offboardX') rate = rospy.Rate(20) mavros.set_namespace('/mavros') state_sub = rospy.Subscriber(mavros.get_topic('state'), mavros_msgs.msg.State, state_callback) arming = rospy.ServiceProxy('/mavros/cmd/arming', mavros_msgs.srv.CommandBool) #set_mode=rospy.ServiceProxy('/mavros/set_mode',SetMode) set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode) #local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'),mavros_msgs.msg.PositionTarget,queue_size=10) local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'), mavros_msgs.msg.PositionTarget, queue_size=10) pose = PositionTarget() pose.header = Header() pose.header.frame_id = "att_pose" pose.header.stamp = rospy.Time.now() pose.position.x = 0 pose.position.y = 0 pose.position.z = 15 while (not state.connected): rate.sleep() for i in range(0, 50): local_pub.publish(pose) #mavros.command.arming(True) #set_mode(0,'OFFBOARD') last_request = rospy.Time.now() while 1: if (state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0))): print("inside22") if (set_mode(0, 'OFFBOARD').success): print("Offboard enabled") last_request = rospy.Time.now() else: if (not state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0))): if (mavros.command.arming(True)): print("vehicle armed") last_request = rospy.Time.now() print("entered") local_pub.publish(pose) rospy.spin() rate.sleep() return 0
def construct_target(self, x, y, z, yaw, yaw_rate=1): target_raw_pose = PositionTarget() target_raw_pose.header = Header() target_raw_pose.header.stamp = rospy.Time.now() target_raw_pose.coordinate_frame = 9 target_raw_pose.position.x = x target_raw_pose.position.y = y target_raw_pose.position.z = z target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \ + PositionTarget.FORCE target_raw_pose.yaw = yaw target_raw_pose.yaw_rate = yaw_rate return target_raw_pose
def main(args): global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target global global_obs_detected, list_velocity_angle, vController par.CurrentIteration = 1 if (len(args) > 1): uav_ID = str(args[1]) else: uav_ID = "1" idx = int(uav_ID) - 1 rospy.init_node("control_uav_" + uav_ID) print "UAV" + uav_ID vController = VelocityController() try: rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming") rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode") except rospy.ROSException: fail("failed to connect to service") state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state", State, queue_size=10, callback=state_cb) local_vel_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_velocity/cmd_vel", TwistStamped, queue_size=10) local_pos_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_position/local", PoseStamped, queue_size=10) local_pos_target = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_raw/local", PositionTarget, queue_size=10) atittude_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_raw/attitude", AttitudeTarget, queue_size=10) thr_pub = rospy.Publisher("/uav" + uav_ID + "/mavros/setpoint_attitude/att_throttle", Float64, queue_size=10) Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid", GridCells, queue_size=10) Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path", GridCells, queue_size=10) arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming", CommandBool) set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode", SetMode) for i in range(4): #velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i) position_sub = rospy.Subscriber("/uav" + repr(i + 1) + "/mavros/local_position/pose", PoseStamped, queue_size=10, callback=position_cb, callback_args=(i, int(uav_ID) - 1)) scan_lidar_sub = rospy.Subscriber("/scan", LaserScan, queue_size=10, callback=scan_lidar_cb, callback_args=uav_ID) br = tf.TransformBroadcaster() r = rospy.Rate(10) print "TRY TO CONNECT" while ((not rospy.is_shutdown()) and (not current_state.connected)): #rospy.spinOnce() r.sleep() #print(current_state.connected.__class__) rospy.loginfo("CURRENT STATE CONNECTED") poses = PoseStamped() #poses.pose = Pose() #poses.pose.position = Point() target = Pose() if (idx == 0): target.position.x = 0 target.position.y = 0 if (idx == 1): target.position.x = 0 target.position.y = 0 if (idx == 2): target.position.x = 0 target.position.y = 0 if (idx == 3): target.position.x = 4 target.position.y = 4 target.position.z = 10 poses.pose.position.x = target.position.x poses.pose.position.y = target.position.y poses.pose.position.z = target.position.z q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0) poses.pose.orientation.x = q[0] poses.pose.orientation.y = q[1] poses.pose.orientation.z = q[2] poses.pose.orientation.w = q[3] i = 100 #while((not rospy.is_shutdown()) and (i>0)): # local_pos_pub.publish(poses) # i = i-1 rviz_visualization_start = False last_request = rospy.Time.now() count = 0 if (idx == 1): target.position.x = 28 target.position.y = 28 if (idx == 2): target.position.x = 0 target.position.y = 17 #thread1 = Thread(target = key_function) #thread1.start() style.use('fivethirtyeight') thread2 = Thread(target=plot_error_live) thread2.start() thread3 = Thread(target=calculate_and_publish_wall, args=(Astar_grid_pub, Astar_path_pub, idx)) thread3.start() #file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w') #file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w') #file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w') error_time = 0 print poses uav_color = [255, 0, 0, 255] topic = 'uav_marker_' + uav_ID uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10) uav_marker = Marker() uav_marker.header.frame_id = "world" uav_marker.type = uav_marker.SPHERE uav_marker.action = uav_marker.ADD uav_marker.scale.x = par.ws_model['robot_radius'] * 3 uav_marker.scale.y = par.ws_model['robot_radius'] * 3 uav_marker.scale.z = 0.005 * par.RealScale uav_marker.color.r = float(uav_color[0]) / 255 uav_marker.color.g = float(uav_color[1]) / 255 uav_marker.color.b = float(uav_color[2]) / 255 uav_marker.color.a = float(uav_color[3]) / 255 uav_marker.pose.orientation.w = 1.0 uav_marker.pose.position.x = 0 #init_pos[0] uav_marker.pose.position.y = 0 #init_pos[1] uav_marker.pose.position.z = 0 #init_pos[2] uav_marker_publisher.publish(uav_marker) uav_color = [255, 255, 255, 255] topic = 'uav_goal_marker_' + uav_ID uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10) uav_goal_marker = Marker() uav_goal_marker.header.frame_id = "world" uav_goal_marker.type = uav_goal_marker.SPHERE uav_goal_marker.action = uav_goal_marker.ADD uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2 uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2 uav_goal_marker.scale.z = 0.005 * par.RealScale uav_goal_marker.color.r = float(uav_color[0]) / 255 uav_goal_marker.color.g = float(uav_color[1]) / 255 uav_goal_marker.color.b = float(uav_color[2]) / 255 uav_goal_marker.color.a = float(uav_color[3]) / 255 uav_goal_marker.pose.orientation.w = 1.0 uav_goal_marker.pose.position.x = 0 #init_pos[0] uav_goal_marker.pose.position.y = 0 #init_pos[1] uav_goal_marker.pose.position.z = 0 #init_pos[2] uav_goal_marker_publisher.publish(uav_goal_marker) uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) last_request_rviz = rospy.Time.now() last_HRVO_request = rospy.Time.now() while (not rospy.is_shutdown()): if (rviz_visualization_start and (rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))): publish_uav_position_rviz( br, list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z, uav_ID) uav_marker = update_uav_marker( uav_marker, (list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z, 1.0)) uav_marker_publisher.publish(uav_marker) last_request_rviz = rospy.Time.now() if ((not rviz_visualization_start) and (current_state.mode != 'OFFBOARD') and (rospy.Time.now() - last_request > rospy.Duration(1.0))): offb_set_mode = set_mode_client(0, 'OFFBOARD') if (offb_set_mode.mode_sent): rospy.loginfo("OFFBOARD ENABLED") last_request = rospy.Time.now() else: if ((not current_state.armed) and (rospy.Time.now() - last_request > rospy.Duration(1.0))): arm_cmd = arming_client(True) if (arm_cmd.success): rospy.loginfo("VEHICLE ARMED") rviz_visualization_start = True last_request = rospy.Time.now() #print distance3D(cur_pose.pose,target) if (distance3D(cur_pose.pose, target) < 0.5) and (not usingvelocitycontrol): print "sampai" usingvelocitycontrol = True usingpositioncontrol = False target = Pose() if (idx == 0): target.position.x = 28 target.position.y = 28 if (idx == 1): target.position.x = 0 target.position.y = 0 if (idx == 2): target.position.x = 21 target.position.y = 14 if (idx == 3): target.position.x = 0 target.position.y = 0 target.position.z = 10 print target psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x) diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints) diagram4.walls = [] for i in range(par.NumberOfPoints ): # berikan wall pada algoritma pathfinding A* for j in range(par.NumberOfPoints): if (par.ObstacleRegion[j, i] == 0): diagram4.walls.append((i, j)) for i in range(par.NumberOfPoints): for j in range(par.NumberOfPoints): if (par.ObstacleRegionWithClearence[j][i] == 0): diagram4.weights.update({(i, j): par.UniversalCost}) goal_pos = (target.position.x / par.RealScale, target.position.y / par.RealScale) start_pos = (cur_pose.pose.position.x / par.RealScale, cur_pose.pose.position.y / par.RealScale) pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4) print pathx print pathy vController.setHeadingTarget(deg2rad(90.0)) #45.0)) target.position.x = pathx[0] * par.RealScale target.position.y = pathy[0] * par.RealScale uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) vController.setTarget(target) vController.setPIDGainX(1, 0.0, 0.0) vController.setPIDGainY(1, 0.0, 0.0) vController.setPIDGainZ(1, 0, 0) vController.setPIDGainPHI(1, 0.0, 0.0) vController.setPIDGainTHETA(1, 0.0, 0.0) vController.setPIDGainPSI(1, 0.0, 0.0) if (usingpositioncontrol): print "kontrol posisi" poses.pose.position.x = 29 poses.pose.position.y = 29 poses.pose.position.z = 10 local_pos_pub.publish(poses) elif (usingvelocitycontrol): if (distance2D(cur_pose.pose, target) < 1.5): if (len(pathx) > 1): del pathx[0] del pathy[0] target.position.x = pathx[0] * par.RealScale target.position.y = pathy[0] * par.RealScale uav_goal_marker = update_uav_marker( uav_goal_marker, (target.position.x, target.position.y, target.position.z, 1.0)) uav_goal_marker_publisher.publish(uav_goal_marker) print target vController.setTarget(target) if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)): last_HRVO_request = rospy.Time.now() header.stamp = rospy.Time.now() des_vel = vController.update(cur_pose) current_agent_pose = ( list_current_position[idx].pose.position.x, list_current_position[idx].pose.position.y, list_current_position[idx].pose.position.z) current_agent_vel = (list_current_velocity[idx].twist.linear.x, list_current_velocity[idx].twist.linear.y) current_neighbor_pose = [] for i in range(par.NumberOfAgents): if (i != idx): current_neighbor_pose.append( (list_current_position[i].pose.position.x, list_current_position[i].pose.position.y)) current_neighbor_vel = [] for i in range(par.NumberOfAgents): if (i != idx): current_neighbor_vel.append( (list_current_velocity[i].twist.linear.x, list_current_velocity[i].twist.linear.y)) V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y) quat_arr = np.array([ list_current_position[idx].pose.orientation.x, list_current_position[idx].pose.orientation.y, list_current_position[idx].pose.orientation.z, list_current_position[idx].pose.orientation.w ]) att = euler_from_quaternion(quat_arr, 'sxyz') #V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2]) V, RVO_BA_all = locplan.RVO_update_single_static( current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par) V_msg = des_vel V_msg.twist.linear.x = V[0] V_msg.twist.linear.y = V[1] local_vel_pub.publish(V_msg) goal = (target.position.x, target.position.y, target.position.z) x = current_position list_error[0].append(error_time) list_error[1].append(goal[0] - x[0]) list_error[2].append(goal[1] - x[1]) list_error[3].append(goal[2] - x[2]) error_time = error_time + 1 k = 0.1 vx = k * (goal[0] - x[0]) vy = k * (goal[1] - x[1]) vz = k * (goal[2] - x[2]) postar = PositionTarget() postar.header = header postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED p = PositionTarget().IGNORE_PX | PositionTarget( ).IGNORE_PY | PositionTarget().IGNORE_PZ a = PositionTarget().IGNORE_AFX | PositionTarget( ).IGNORE_AFY | PositionTarget().IGNORE_AFZ v = PositionTarget().IGNORE_VX | PositionTarget( ).IGNORE_VY | PositionTarget().IGNORE_VZ y = PositionTarget().IGNORE_YAW | PositionTarget( ).IGNORE_YAW_RATE f = PositionTarget().FORCE postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE postar.velocity.x = vx postar.velocity.y = vy postar.velocity.z = vz postar.position.x = goal[0] postar.position.y = goal[1] postar.position.z = goal[2] vel_msg = TwistStamped() vel_msg.header = header vel_msg.twist.linear.x = xvel vel_msg.twist.linear.y = yvel vel_msg.twist.linear.z = 0.0 vel_msg.twist.angular.x = 0.0 vel_msg.twist.angular.y = 0.0 vel_msg.twist.angular.z = 0.0 q = quaternion_from_euler(0, 0, 0.2) att = PoseStamped() att.pose.orientation.x = q[0] att.pose.orientation.y = q[1] att.pose.orientation.z = q[2] att.pose.orientation.w = q[3] att.pose.position.x = 0.0 att.pose.position.y = 0.0 att.pose.position.z = 2.0 cmd_thr = Float64() cmd_thr.data = 0.3 att_raw = AttitudeTarget() att_raw.header = Header() att_raw.body_rate = Vector3() att_raw.thrust = 0.7 #Float64() att_raw.header.stamp = rospy.Time.now() att_raw.header.frame_id = "fcu" att_raw.type_mask = 71 att_raw.orientation = Quaternion( *quaternion_from_euler(0, 0, 0.2)) else: local_pos_pub.publish(poses) count = count + 1 r.sleep() try: rospy.spin() except KeyboardInterrupt: print("Shutting down") cv2.destroyAllWindows()