def __init__(self, mav_id, mav_num): self.arm_state = False self.offboard_state = False self.state = None self.command = TwistStamped() self.start_point = PoseStamped() self.feb_pos = PoseStamped() self.feb_bias_pos = Point32() self.obs = Point32() self.obs_vel = Point32() self.drone_id = mav_id self.drone_num = mav_num self.is_attack = False self.state_drone = UInt64() self.attack_num = UInt64() self.attack_pos = [195, 10, 1.5] mav_x = [0, -30, 0] mav_y = [60, 30, 0] self.start_point.pose.position.x = mav_x[self.drone_id - 1] self.start_point.pose.position.y = mav_y[self.drone_id - 1] self.start_point.pose.position.z = 4 ''' ros publishers ''' self.vel_pub = rospy.Publisher( '/drone_%s/mavros/setpoint_velocity/cmd_vel' % (self.drone_id), TwistStamped, queue_size=10) self.drone_state_pub = rospy.Publisher("/drone_%s/state" % (self.drone_id), UInt64, queue_size=10) ''' ros subscriber ''' # self.local_pos_sub = rospy.Subscriber('/drone_%s/mavros/setpoint_position/local'%(self.drone_id), PoseStamped, self.local_pos_cb) self.bias_pos_sub = rospy.Subscriber( '/drone_%s/mavros/local_position/pose_cor' % (self.drone_id), PoseStamped, self.bias_cb) self.obs_sub = rospy.Subscriber("ue4_ros/drone_3/pos", Point32, self.obj_cb) self.state_sub = rospy.Subscriber("/drone_%s/state" % (self.drone_id), UInt64, self.state_cb) self.attack_key = rospy.Subscriber("/is_attrack", UInt64, self.attack_cb) ''' ros services ''' self.armService = rospy.ServiceProxy( '/drone_%s/mavros/cmd/arming' % (self.drone_id), CommandBool) self.flightModeService = rospy.ServiceProxy( '/drone_%s/mavros/set_mode' % (self.drone_id), SetMode) self.takeoffService = rospy.ServiceProxy( '/drone_%s/mavros/cmd/takeoff' % (self.drone_id), CommandTOL) print("obj Controller Initialized!")
def test_dictionary_with_uint64(self): from std_msgs.msg import UInt64 expected_message = UInt64(data = 0xFFFFFFFFFFFFFFFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt64', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def test_ros_message_with_uint64(self): from std_msgs.msg import UInt64 expected_dictionary = { 'data': 0xFFFFFFFFFFFFFFFF } message = UInt64(data=expected_dictionary['data']) message = serialize_deserialize(message) dictionary = message_converter.convert_ros_message_to_dictionary(message) self.assertEqual(dictionary, expected_dictionary)
def start(self): global drone_state, drone_state_pub cnt_pipe = 0 while not rospy.is_shutdown(): pipes = self.play["Drone1"] len_pipe = len(pipes) if drone_state == 20: a = Pipeline() a.pipetype.data = pipes[cnt_pipe]["pipetype"] for u in pipes[cnt_pipe]["units"]: b = Pipeunit() b.middle = Point32(u[0][0], u[0][1], u[0][2]) b.left = Point32(u[1][0], u[1][1], u[1][2]) b.right = Point32(u[2][0], u[2][1], u[2][2]) if len(u) > 3: b.bottom = Point32(u[3][0], u[3][1], u[3][2]) b.up = Point32(u[4][0], u[4][1], u[4][2]) a.units.append(b) print(a) cnt_pipe += 1 drone_state = 30 drone_state_pub.publish(UInt64(drone_state)) self.pipe_pub.publish(a) if cnt_pipe >= len_pipe: break self.rate.sleep()
def callback(data): dipswitch_value1 = 0 dipswitch_value2 = 0 dipswitch_value3 = 0 dipswitch_value4 = 0 for pin in data.pins: if pin.pinNumber == DIPSWITCH_PIN1: dipswitch_value1 = int(pin.pinValue) elif pin.pinNumber == DIPSWITCH_PIN2: dipswitch_value2 = int(pin.pinValue) elif pin.pinNumber == DIPSWITCH_PIN3: dipswitch_value3 = int(pin.pinValue) elif pin.pinNumber == DIPSWITCH_PIN4: dipswitch_value4 = int(pin.pinValue) # Dipswitch have pin 1 at left, so it will be MSB userid = 0 userid += dipswitch_value1 << 3 userid += dipswitch_value2 << 2 userid += dipswitch_value3 << 1 userid += dipswitch_value4 << 0 msg = UInt64() msg = userid pub.publish(userid)
def idle(self): print("I'm in idle state!") global drone_state, drone_state_pub idle_cmd = TwistStamped() while not rospy.is_shutdown(): if drone_state == 40: self.vel_pub.publish(idle_cmd) drone_state_pub.publish(UInt64(40)) self.rate.sleep()
def __init__(self): self.arm_state = False self.offboard_state = False self.state = None self.mav_pos = PoseStamped() self.start_sphere = Point32() self.mav_feb = Point32() self.obj_pos = Point32() self.attack_num = UInt64() self.sphere_msg = Obj() self.people_msg = Obj() self.sphere_msg.id = 50 self.sphere_msg.type = 152 self.sphere_msg.size.x = 0.1 self.sphere_msg.size.y = 0.1 self.sphere_msg.size.z = 0.1 self.sphere_msg.position.x = 195 #0 self.sphere_msg.position.y = 10 #30 self.sphere_msg.position.z = -0.1 #0 self.people_msg.id = 60 self.people_msg.type = 30 self.people_msg.position.x = 195 #0 self.people_msg.position.y = 10 #30 self.people_msg.position.z = 0 #0 self.people_msg.angule.z = 0 self.people_msg.size.x = 1 self.people_msg.size.y = 1 self.people_msg.size.z = 1 self.start_sphere.x = self.people_msg.position.x self.start_sphere.y = self.people_msg.position.y self.start_sphere.z = self.people_msg.position.z ''' ros publishers ''' self.obj_pub = rospy.Publisher("ue4_ros/obj", Obj, queue_size=10) self.obj_pos_pub = rospy.Publisher("ue4_ros/drone_3/pos", Point32, queue_size=10) self.attack_key = rospy.Subscriber("/is_attrack", UInt64, self.attack_cb) ''' ros subscriber ''' # self.local_sub = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.local_pos_cb) self.bias_pos_sub = rospy.Subscriber( '/drone_1/mavros/local_position/pose_cor', PoseStamped, self.bias_cb) print("obj Controller Initialized!")
def start(self): rate = rospy.Rate(20) for _ in range(10): self.vel_pub.publish(self.command) self.arm_state = self.arm() self.offboard_state = self.offboard() rate.sleep() desire = [0, 0, 0] while (rospy.is_shutdown() is False): # target = np.array([self.start_point.pose.position.x, self.start_point.pose.position.y, \ # self.start_point.pose.position.z]) feb = np.array([self.feb_bias_pos.x, self.feb_bias_pos.y, \ self.feb_bias_pos.z]) # if self.is_attack == True: # if self.state_drone_1 == 40 and self.state_drone_2 == 40 and self.state_drone_3 == 40: # self.is_attack == True if self.attack_num.data == 20: self.is_attack = True if self.is_attack == True: drone_state = 50 self.drone_state_pub.publish(UInt64(drone_state)) target = np.array([ self.attack_pos[0], self.attack_pos[1], self.attack_pos[2] ]) desire = self.pos_control(target, feb, 0.8, 5) else: desire[0] = 0 desire[1] = 0 desire[2] = 0 self.obs_vel.x = 0 self.obs_vel.y = 0 self.obs_vel.z = 0 direct_obs = -1 #(self.feb_bias_pos.x - self.obs.x)/abs(self.feb_bias_pos.x - self.obs.x) if abs(self.feb_bias_pos.x - self.obs.x) < 8 and self.drone_id == 1: self.obs_vel.y = direct_obs * 2 self.command.twist.linear.x = desire[0] + self.obs_vel.x self.command.twist.linear.y = desire[1] + self.obs_vel.y self.command.twist.linear.z = desire[2] + self.obs_vel.z self.command.twist.angular.z = 0 if self.state_drone.data == 50: self.vel_pub.publish(self.command) rate.sleep() rospy.spin()
def service_handler(self, input): print('service call received') response = InfoGainsResponse() ros_image = input.multi_channel_occupancy_grid np_arr = np.fromstring(ros_image.data, np.uint8).reshape(ros_image.height, ros_image.width, 3) print('image size', np_arr.shape) cv2.imwrite('/tmp/costmap_srv.png', np_arr) frontiers_data = OneDataInfoBase( np_arr, self._convert_frontier_info({ 'BoundingBoxes': input.frontier_rois, 'Frontiers': input.frontier_clusters })) response.info_gains = [] batch_input = [] batch_info = [] for i in range(len(frontiers_data)): image, info = frontiers_data[i] image = torch.from_numpy(image) batch_input.append(image) batch_info.append(info) batch_input = torch.stack(batch_input) batch_input = Variable(batch_input) if self.args.cuda: batch_input = batch_input.cuda() output, _, _ = self.model(batch_input) info_gain = compute_expected_information_gain(batch_input, output, batch_info, 'info_gain_srv') for i in range(len(info_gain)): msg = UInt64(info_gain[i]['information_gain']) response.info_gains.append(msg) return response
def calc_eta(self, event): """ Calculates the Estimated Time of Arrival to the destination """ # Attempt an update only if the cart is driving if self.current_state.is_navigating: # Where are we at and how much further must we go current_node = self.get_closest_point(self.global_pose.position.x, self.global_pose.position.y) distance_remaining = self.calc_trip_dist(self.local_points, current_node) # Remaining time in seconds remaining_time = distance_remaining / self.cur_speed eta_msg = UInt64() # Calculate the ETA to the end arrival_time = time.time() + remaining_time # Convert the time to milliseconds eta_msg.data = int(arrival_time * (1000)) self.eta_pub.publish(eta_msg)
def string2uint64(self, in_string): ans = UInt64(uuid.UUID(in_string).int) return ans
def __init__(self): self.TSData = UInt64() self.button_input = [0 for i in range(11)] self.axes_input = [0 for i in range(8)] rospy.Subscriber('/joy', Joy, self.Joystick_CallBack) self.TSPub = rospy.Publisher('/ThrusterStates', UInt64, queue_size = 10)
drone_state_pub.publish(UInt64(drone_state)) self.pipe_pub.publish(a) if cnt_pipe >= len_pipe: break self.rate.sleep() def drone_state_cb(msg): global drone_state drone_state = msg.data if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('-p', '--play', default=os.path.join(os.path.expanduser('~'),"Swarm_ws/src/decision/scenarios","play.json"), help='play file path') args = parser.parse_args() print(args) play_file = open(args.play) play = json.load(play_file) print(json.dumps(play)) rospy.init_node('decision_node', anonymous=True) drone_state_sub = rospy.Subscriber('drone_1/state', UInt64, drone_state_cb) drone_state_pub = rospy.Publisher('drone_1/state', UInt64, queue_size=10) drone_state = 10 drone_state_pub.publish(UInt64(drone_state)) px4 = Px4Controller() px4.start() drone_state = 20 drone_state_pub.publish(UInt64(drone_state)) dd = Decision(play) dd.start() print("Finish")
def __init__(self): self.imu = None self.gps = None self.local_pose = None self.current_state = None self.current_heading = None self.local_enu_position = None self.arm_state = False self.offboard_state = False self.received_imu = False self.frame = "BODY" self.imu_rate = np.array([0, 0, 0]) self.state = None self.mavros_state = State() self.command = TwistStamped() self.drone_state = UInt64() # self.drone_state = String() self.mav_pos = np.array([0, 0, 0]) self.mav_vel = np.array([0, 0, 0]) self.cmd_vel = np.array([0, 0, 0]) self.mav_yaw = 0 self.cmd_yaw = 0 self.mav_roll = 0 self.mav_pitch = 0 self.mav_R = np.zeros((3, 3)) #第一个任务起始点位置 self.initial_pos = np.array([147, -8 - 5, 4]) #搜索点位置 self.search_pos = np.array([[150, 3, 4], [180, 4.5, 4], [244, 1.2, 4], [244, 20, 4], [160, 18, 4], [170, 11, 4]]) self.search_point = 0 self.target_pos = np.array([200, 10, 4]) self.dis_initial_pos = 2 self.dis_search_pos = 1 self.dis_target_pos = 1 self.P_pos = 0.8 self.P_yaw = 0.5 self.pos_vel_sat = 5 self.yaw_sat = 0.8 ''' ros subscribers /balance_force /ttc_mean ''' self.local_pose_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.local_pose_callback) self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback) self.gps_sub = rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_callback) self.imu_sub = rospy.Subscriber("/mavros/imu/data", Imu, self.imu_callback) self.mav_sub = rospy.Subscriber("mavros/local_position/velocity_local", TwistStamped, self.mav_vel_cb) self.drone_state_sub = rospy.Subscriber('drone_1/state', UInt64, self.drone_state_cb) # self.imu_sub = rospy.Subscriber("mavros/imu/data", Imu, self.mav_vel_cb) self.is_offboard = False self.is_start, self.is_tunnel, self.is_trapezoid, self.is_search = False, False, False, False ''' ros publishers ''' self.vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10) # self.drone_state_pub = rospy.Publisher('drone_1/state', String, queue_size=10) self.drone_state_pub = rospy.Publisher('drone_1/state', UInt64, queue_size=10) self.task_takeoff = 10 self.task_out_search = 20 self.task_recognize_door = 30 self.task_pass_through = 40 self.task_in_search = 50 self.task_recognize_target = 60 self.task_attack = 70 self.task_finish = 80 ''' caes1:"task_takeoff" caes2:"task_out_search" caes3:"task_recognize_door" caes4:"task_pass_through" caes5:"task_in_search" caes6:"task_recognize_target" caes7:"task_attack" caes8:"task_finish" ''' ''' ros services ''' self.armService = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool) self.flightModeService = rospy.ServiceProxy('/mavros/set_mode', SetMode) print("Px4 Controller Initialized!")
state_1 = msg def drone2_state_cb(msg): global state_2 state_2 = msg def drone3_state_cb(msg): global state_3 state_3 = msg if __name__ == '__main__': rospy.init_node('is_attack_node', anonymous=True) state_1 = UInt64() state_2 = UInt64() state_3 = UInt64() attack = UInt64() drone1_state_sub = rospy.Subscriber("/drone_1/state", UInt64, drone1_state_cb) drone2_state_sub = rospy.Subscriber("/drone_2/state", UInt64, drone2_state_cb) drone3_state_sub = rospy.Subscriber("/drone_3/state", UInt64, drone3_state_cb) drone_state_pub = rospy.Publisher("/is_attrack", UInt64, queue_size=10) rate = rospy.Rate(20) while (rospy.is_shutdown() is False): if state_1.data == 40 and state_2.data == 40 and state_3.data == 40: attack.data = 20
continue try: resp = ServerProxy(node_api).getPid('/NODEINFO') except: rospy.logerr("[cpu monitor] failed to get pid of node %s (api is %s)" % (node, node_api)) else: node_map[node] = Node(node, resp[2]) rospy.loginfo("[cpu monitor] adding new node %s" % node) for node_name, node in list(node_map.items()): if node.alive(): node.publish() else: rospy.logwarn("[cpu monitor] lost node %s" % node_name) del node_map[node_name] cpu_publish.publish(Float32(psutil.cpu_percent())) cpu_id = [4,6,9,11] for idx,pt in enumerate(temp_publishers): id = "tsens_tz_sensor" + str(cpu_id[idx]) pt.publish(Float32(psutil.sensors_temperatures()[id][0].current * 100)) vm = psutil.virtual_memory() for mem_topic, mem_publisher in zip(mem_topics, mem_publishers): mem_publisher.publish(UInt64(getattr(vm, mem_topic))) rospy.sleep(POLL_PERIOD)
def publish(self): self.cpu_publisher.publish(Float32(self.proc.cpu_percent())) self.mem_publisher.publish(UInt64(self.proc.memory_info().rss))
def pub_time(self): msg = UInt64() msg.data = self.time_elapsed self.p_time.publish(msg)