def talker():
    pub = rospy.Publisher("mavros/rc/override", OverrideRCIn, queue_size=10)
    rospy.init_node("custom_talker", anonymous=True)
    r = rospy.Rate(10)  # 10hz
    msg = OverrideRCIn()
    start = time.time()
    flag = True  # time flag
    # for i in range (0,8):
    #        msg.channels[i]=2000
    # speed='SLOW'

    direction = "LEFT"
    if direction == "CENTER":
        msg.channels[steer_channel] = 1385
    elif direction == "RIGHT":
        msg.channels[steer_channel] = 1450
    elif direction == "LEFT":
        msg.channels[steer_channel] = 1300

    msg.channels[throttle_channel] = 1558
    while not rospy.is_shutdown() and flag:
        sample_time = time.time()
        if (sample_time - start) > exec_time:
            flag = False
        rospy.loginfo(msg)
        pub.publish(msg)
        r.sleep()
 def callback(self, data):
     pitch = -data.axes[0] * 500 + 1500
     forward = -data.axes[1] * 500 + 1500
     throttle = data.axes[2] * 500 + 1500
     msg = OverrideRCIn()
     msg.channels[0] = pitch
     msg.channels[1] = forward
     msg.channels[2] = throttle
     msg.channels[3] = 1500
     self.pub.publish(msg) 
    def real_publish(self,desired_3d_force_quad,yaw_rate,rc_output):

        # ORDER OF INPUTS IS VERY IMPORTANT: roll, pitch, throttle,yaw_rate
        # create message of type OverrideRCIn
        rc_cmd          = OverrideRCIn()
        other_channels  = numpy.array([1500,1500,1500,1500])
        channels        = numpy.concatenate([rc_output,other_channels])
        channels        = numpy.array(channels,dtype=numpy.uint16)
        rc_cmd.channels = channels
        self.rc_override.publish(rc_cmd)     
Exemple #4
0
def manualControl(throttle , speed, steering, steer_intensity):
	pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
	rospy.init_node('custom_talker', anonymous=True)
	r = rospy.Rate(10) #10hz
	msg = OverrideRCIn()
		
	#Set channels
	throttle_channel = 1	
	steer_channel = 3	
	
	#Normalize speed	
	if speed >= 0 and speed <=100:	
		speed = speed * 5
	else:
		print 'Speed out of range (0~100).'
		return

	#Set direction
	if throttle == 'FORWARD':
		throttle = 1500 + speed
	elif throttle == 'BACKWARD':
		throttle = 1500 - speed
	elif throttle == 'NONE':
		throttle = 1500	
	else:
		print 'Unknown throttle. Use \'FORWARD\', \'BACKWARD\' or \'NONE\').'
		return		

	#Normalize turn intensity	
	if steer_intensity >= 0 and steer_intensity <=100:
		steer_intensity = steer_intensity * 5
	else:
		print 'Turn intensity out of range (0~100).'
		return
		
	#Set steering
	if steering == 'RIGHT':
		steering = 1500 + steer_intensity
	elif steering == 'LEFT':
		steering = 1500 - steer_intensity
	elif steering == 'NONE':
		steering = 1500
	else:
		print 'Unknown direction. Use \'RIGHT\', \'LEFT\' or \'NONE\').'
		return
	
	#Set throttle
	msg.channels[throttle_channel] = throttle

	#Set orientation
	msg.channels[steer_channel] = steering

	rospy.loginfo(msg)
	pub.publish(msg)
	r.sleep()
def talker():
	pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
    	rospy.init_node('custom_talker', anonymous=True)
	r = rospy.Rate(10) #10hz
	msg = OverrideRCIn()
	start = time.time()
        flag=True #time flag
	#for i in range (0,8):
        #        msg.channels[i]=2000
	#speed='SLOW'
	speed='FAST'
	if speed =='SLOW':
		msg.channels[throttle_channel]=1560
		msg.channels[steer_channel]=1370
	elif speed =='NORMAL':
		msg.channels[throttle_channel]=1565
                msg.channels[steer_channel]=1370
	elif speed == 'FAST':	
		msg.channels[throttle_channel]=1570
                msg.channels[steer_channel]=1370
	while not rospy.is_shutdown() and flag:
		sample_time=time.time()
		if ((sample_time - start) > exec_time):
			flag=False
        	rospy.loginfo(msg)
        	pub.publish(msg)
        	r.sleep()
Exemple #6
0
def talker():
 pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
 r = rospy.Rate(10) #10hz
 msg = OverrideRCIn()
 start = time.time()
 speed='SLOW'
 exec_time=2
 flag=True #time flag
 if speed =='SLOW':
  msg.channels[throttle_channel]=1558
 elif speed =='NORMAL':
  msg.channels[throttle_channel]=1565
 elif speed == 'FAST':
  msg.channels[throttle_channel]=1570
 direction='STRAIGHT'
 if direction =='STRAIGHT':
  msg.channels[steer_channel]=1385
 elif direction =='RIGHT':
  msg.channels[steer_channel]=1450
 elif direction == 'LEFT':
  msg.channels[steer_channel]=1300
 while not rospy.is_shutdown() and flag:
  sample_time=time.time()
  if ((sample_time - start) > exec_time):
   flag=False
  rospy.loginfo(msg)
  pub.publish(msg)
  r.sleep()
  def callback(self,data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)

    height, width, channels = cv_image.shape
    crop_img = cv_image[200:(height)/2+150][1:width]

    lower = np.array([0, 0, 79], dtype = "uint8")
    upper = np.array([40, 40, 191], dtype = "uint8")

    mask = cv2.inRange(crop_img, lower, upper)
    extraction = cv2.bitwise_and(crop_img, crop_img, mask = mask)
    m = cv2.moments(mask, False)
    try:
      x, y = m['m10']/m['m00'], m['m01']/m['m00']
    except ZeroDivisionError:
      x, y = height/2, width/2
    cv2.circle(extraction,(int(x), int(y)), 2,(0,255,0),3)

    cv2.imshow("Image window", np.hstack([crop_img,extraction]))
    cv2.waitKey(1)

    yaw = 1500 + (x - width/2) * 1.5
    print "center=" + str(width/2) + "point=" + str(x) + "yaw=" +  str(yaw)
    throttle = 1900

    if (yaw > 1900):
      yaw = 1900
    elif (yaw < 1100):
      yaw = 1100
        
    msg = OverrideRCIn()
    msg.channels[0] = yaw
    msg.channels[1] = 0
    msg.channels[2] = throttle
    msg.channels[3] = 0
    msg.channels[4] = 0
    msg.channels[5] = 0
    msg.channels[6] = 0
    msg.channels[7] = 0
    self.pub.publish(msg)
def callback(data):
    frame = np.zeros((500, 500,3), np.uint8)
    angle = data.angle_max
    Vx = 250
    Vy = 250
    for r in data.ranges:
        if r == float ('Inf'):
            r = data.range_max
        x = math.trunc( (r * 10)*math.cos(angle + (-90*3.1416/180)) )
        y = math.trunc( (r * 10)*math.sin(angle + (-90*3.1416/180)) )
        cv2.line(frame,(250, 250),(x+250,y+250),(255,0,0),1)
        Vx+=x
        Vy+=y
        angle= angle - data.angle_increment

    cv2.line(frame,(250, 250),(250+Vx,250+Vy),(0,0,255),3)
    cv2.circle(frame, (250, 250), 2, (255, 255, 0))
    ang = -(math.atan2(Vx,Vy)-3.1416)*180/3.1416
    if ang > 180:
        ang -= 360
    cv2.putText(frame,str(ang)[:10], (50,400), cv2.FONT_HERSHEY_SIMPLEX, 2, (255,255,255))

    cv2.imshow('frame',frame)
    cv2.waitKey(1)
    
    yaw = 1500 + ang * 40 / 6
    throttle = 1900
        
    msg = OverrideRCIn()
    msg.channels[0] = yaw
    msg.channels[1] = 0
    msg.channels[2] = throttle
    msg.channels[3] = 0
    msg.channels[4] = 0
    msg.channels[5] = 0
    msg.channels[6] = 0
    msg.channels[7] = 0
    pub.publish(msg)   
Exemple #9
0
def throttle_yaw_pitch_roll(publisher,throttle=1500,yaw=0,pitch=0,roll=0):
	print 'Throttle', throttle
	print 'Yaw', yaw
	print 'pitch', pitch
	print 'roll', roll
	min_val = 1000;
	max_val = 2000;

	if len( filter( lambda x: (x < min_val or x > max_val) and x != 0, [roll, pitch, throttle, yaw] ) ) > 0:
		print 'Values must be between %d and %d' % (min_val, max_val)
		return

	msg = OverrideRCIn()
	msg.channels[0] = roll
	msg.channels[1] = pitch
	msg.channels[2] = throttle
	msg.channels[3] = yaw
	msg.channels[4] = 1100
	msg.channels[5] = 1100
	msg.channels[6] = 1100
	msg.channels[7] = 1100

	publisher.publish(msg)
Exemple #10
0
    def start(self, debug=False):
        rcmsg = OverrideRCIn()
        rcmsg.channels = [
            65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535
        ]
        # 设置程序状态,0 为未设置,1为遥控,2 为延边状态
        first_cortron_state = 0
        # 航点索引
        waypoint_index = 0
        bridge_time = 0
        # 0 向桥首个航点驶入
        # 1 进入直行状态
        # 2 过桥完成,前往过桥后与当前航向不超过70度的航点
        bridge_status = 3
        hum_msg = HumRunning()

        first_arm = True
        first_manual = True
        while True:
            # 切入解锁
            # self.log.logger.info("arm_state:" + str(self.arm_state))
            # self.log.logger.info("manual_state:" + str(self.manual_state))
            if self.arm_state and self.manual_state and (rospy.is_shutdown() is
                                                         False):
                if self.current_gps != None and len(
                        self.rc_in) > 2 and self.rc_in[5] > 1500:
                    if first_cortron_state != 2:
                        rcmsg.channels[2] = 1850

                        self.log.logger.info("延边模式")
                        # 首次切入延边状态进入前往地一个航点
                        first_cortron_state = 2
                        waypoint_index = 0
                        waypoint = self.current_gps

                    # 判断当前位置与航点距离多少
                    self.current_waypoint_dist = gps_utiles.gps_get_distance(
                        self.current_gps, waypoint)

                    self.current_heading = gps_utiles.gps_get_angle(
                        self.current_gps, waypoint)
                    hum_msg.currenrHeading = self.current_heading
                    hum_msg.currenrYaw = self.yaw

                    if bridge_status == 3:
                        min_bridge_waypoints = self.get_min_bridge()
                        # 获取距离当前位置与最近桥的距离以及桥本身航点
                        if len(min_bridge_waypoints) > 0:
                            bridge_heading = gps_utiles.gps_get_angle(
                                min_bridge_waypoints[0],
                                min_bridge_waypoints[-1])
                            bridge_distance = gps_utiles.gps_get_distance(
                                min_bridge_waypoints[0],
                                min_bridge_waypoints[-1])
                            self.log.logger.info("开始进行过桥,桥长" +
                                                 str(bridge_distance) +
                                                 "米 ,桥起始点: " +
                                                 str(min_bridge_waypoints[0]))
                            # 满足过桥行动
                            bridge_time = 0
                            bridge_status = 0

                        elif self.current_waypoint_dist < self.config.min_waypoint_distance:
                            # print("self.current_waypoint_dist",self.current_waypoint_dist)
                            # print("waypoint_index",waypoint_index)

                            if (waypoint_index <= 1
                                    and self.current_waypoint_dist < 5
                                ) or 1 < waypoint_index < len(
                                    self.kml_dict["left"]):
                                waypoint = self.kml_dict["left"][
                                    waypoint_index]
                                waypoint_index += 1
                                self.log.logger.info("前往第" +
                                                     str(waypoint_index) +
                                                     "个航点:" + str(waypoint))

                            elif waypoint_index == len(self.kml_dict["left"]):
                                # rcmsg.channels[2] = 1500
                                # rcmsg.channels[0] = 1500

                                self.log.logger.info("到达第" +
                                                     str(waypoint_index) +
                                                     "个航点:" + str(waypoint))
                                self.log.logger.info("任务结束")
                                waypoint_index = 0
                                waypoint = self.current_gps

                    # 若当前位置距离桥第一个点小于最小航点距离则进行
                    elif bridge_status == 0:
                        current_bridge_heading = gps_utiles.gps_get_angle(
                            self.current_gps, min_bridge_waypoints[0])
                        current_bridge_distance = gps_utiles.gps_get_distance(
                            self.current_gps, min_bridge_waypoints[0])

                        self.set_yaw_pid_control(current_bridge_heading, rcmsg,
                                                 hum_msg)
                        if current_bridge_distance < self.config.min_waypoint_distance:
                            bridge_status = 1
                            start_time = time.time()

                    elif bridge_status == 1:

                        self.set_yaw_pid_control(bridge_heading, rcmsg,
                                                 hum_msg)
                        bridge_time = time.time() - start_time
                        if bridge_time > (bridge_distance +
                                          5) / self.config.ground_speed:
                            self.log.logger.info("已通过桥,耗时" + str(bridge_time) +
                                                 "秒,桥末尾点: " +
                                                 str(min_bridge_waypoints[-1]))
                            # 确定通过桥梁
                            bridge_status = 2

                    elif bridge_status == 2:
                        if waypoint_index < len(
                                self.kml_dict["left"]
                        ) - 1 and angle_utils.angle_different_0_360(
                                self.current_heading, self.yaw) > 90:
                            waypoint = self.kml_dict["left"][waypoint_index]
                            self.log.logger.info("跳过第" + str(waypoint_index) +
                                                 "个航点:" + str(waypoint))
                            waypoint_index += 1
                        else:
                            self.log.logger.info("前往第" + str(waypoint_index) +
                                                 "个航点:" + str(waypoint))
                            bridge_status = 3

                    # self.log.logger.info( "current_heading: " + str(self.current_heading))
                    # print(len(self.all_points))
                    if not self.config.yaw_PID_debug and bridge_status == 3:
                        self.get_dist_parallel_yaw()
                        # 判断点云是否符合沿岸要求,符合要求是时进行沿岸行走,不符合要求航向向下个航点行驶,或者进入调试模式下不进入延边模式
                        if len(self.all_points) > 130:
                            self.get_line_slope()
                            if self.left_dist != None and self.left_parallel_yaw != None and self.left_dist > 0.5:
                                hum_msg.leftDist = self.left_dist
                                hum_msg.leftParallelYaw = self.left_parallel_yaw

                                # 根据与岸的距离不断调整 yaw 是其不断逼近想要的距离
                                self.set_dist_pid_control(
                                    self.config.except_dist, self.left_dist,
                                    self.left_parallel_yaw, rcmsg, hum_msg)
                            else:
                                self.left_dist = None
                            self.left_parallel_yaw = None
                        else:
                            self.left_dist = None
                            self.left_parallel_yaw = None

                            # 向航点航向行驶
                            self.set_yaw_pid_control(self.current_heading,
                                                     rcmsg, hum_msg)
                            time.sleep(0.1)
                    else:
                        self.left_dist = None
                        self.left_parallel_yaw = None
                        # 向航点航向行驶
                        self.set_yaw_pid_control(self.current_heading, rcmsg,
                                                 hum_msg)
                        time.sleep(0.1)
                # 如果切换到遥控模式
                elif len(self.rc_in) > 2 and self.rc_in[5] < 1500:

                    if first_cortron_state != 1:
                        self.log.logger.info("遥控模式")
                        first_cortron_state = 1
                        rcmsg.channels[2] = 1500

                    rcmsg.channels[2] = 65535
                    # print(self.rc_in)
                    rcmsg.channels[0] = self.rc_in[3]
                    # rcmsg.channels[0] = 65535

                    time.sleep(0.1)
                # self.log.logger.info("rcmsg:" + str(rcmsg))
                hum_msg.config = self.config
                self.rc_override_pub.publish(rcmsg)
                self.hum_msg_pub.publish(hum_msg)
                first_arm = True
                first_manual = True

            if not self.manual_state and first_manual:
                self.log.logger.info("请切换到manual模式")
                first_manual = False

            if not self.arm_state and first_arm:
                self.log.logger.info("请解锁")
                first_arm = False
Exemple #11
0
    def __init__(self, debug=False):

        rospy.init_node('robdos_controller')

        self.degrees2rad = math.pi / 180.0
        self.rad2degrees = 180.0 / math.pi
        self.debug = debug
        self.current_target = None
        self.is_target_ready = False
        self.is_localization_ready = False

        # define rate of  100 hz
        self.rate = rospy.Rate(50.0)

        # init variables for odometry
        [self.robot_position_x, self.robot_position_y,
         self.robot_position_z] = [0, 0, 0]
        [self.robot_roll, self.robot_pitch, self.robot_yaw] = [0, 0, 0]

        # init control variables
        [
            self.threshold_position, self.threshold_orientation,
            self.threshold_depth
        ] = [0.1, 10, 0.1]

        [self.P_kp, self.P_kd, self.P_ki, self.P_windup] = [0.0, 0.0, 0.0, 0.0]
        [self.O_kp, self.O_kd, self.O_ki, self.O_windup] = [0.0, 0.0, 0.0, 0.0]
        [self.D_kp, self.D_kd, self.D_ki, self.D_windup] = [0.0, 0.0, 0.0, 0.0]

        self.position_pid = PID()
        self.orientation_pid = PID()
        self.depth_pid = PID()

        [self.limit_min, self.limit_max] = [0.0, 0.0]
        """initialize position and orientation controller"""
        self.srv = Server(controllerConfig, self.reconfig_callback)

        # event publisher
        self.event_pub = rospy.Publisher("/robdos/stateEvents",
                                         StateEvent,
                                         queue_size=1)
        self.event_msg = StateEvent()

        # mavros velocity publisher
        self.mavros_vel_pub = rospy.Publisher("robdos/autopilot_commands",
                                              OverrideRCIn,
                                              queue_size=1)
        self.RCOR_msg = OverrideRCIn()

        # create subscriber for robot localization
        self.sub_localization = rospy.Subscriber(
            '/robdos/simulatedOdometry',
            Odometry,
            self.process_localization_message,
            queue_size=1)

        self.sub_waypoint = rospy.Subscriber('/robdos/mission/waypoints',
                                             Waypoint,
                                             self.process_waypoint_message,
                                             queue_size=1)

        rospy.wait_for_service('waypoint_reached')
        self.waypointReached = rospy.ServiceProxy('waypoint_reached', Empty)
Exemple #12
0
    def control_compute(self):

        # node will be named quad_control (see rqt_graph)
        rospy.init_node('quad_control', anonymous=True)

        # message published by quad_control to GUI
        self.pub = rospy.Publisher('quad_state_and_cmd',
                                   quad_state_and_cmd,
                                   queue_size=10)

        # message published by quad_control that simulator will subscribe to
        self.pub_cmd = rospy.Publisher('quad_cmd', quad_cmd, queue_size=10)

        # for publishing state of the controller
        self.pub_ctr_st = rospy.Publisher('ctr_state',
                                          Controller_State,
                                          queue_size=10)
        # initialize flag for publishing controller state at false
        self.flagPublish_ctr_st = False

        # controller needs to have access to STATE of the system
        # this can come from QUALISYS, a sensor, or the simulator
        self.SubToSim = rospy.Subscriber("quad_state", quad_state,
                                         self.get_state_from_simulator)

        #-----------------------------------------------------------------------#
        # TO SAVE DATA FLAG
        # by default, NO data is saved
        self.SaveDataFlag = False
        # we will use this just for convenience, when generating the names of the files
        # where the data will be saved
        self.TimeSaveData = rospy.get_time()
        # Service is created, so that data is saved when GUI requests
        Save_data_service = rospy.Service('SaveDataFromGui', SaveData,
                                          self.handle_Save_Data)

        #-----------------------------------------------------------------------#
        # SERVICE FOR SELECTING DESIRED TRAJECTORY
        # by default, STAYING STILL IN ORIGIN IS DESIRED TRAJECTORY
        # self.flagTrajDes = 0
        # Service is created, so that data is saved when GUI requests
        TrajDes_service = rospy.Service('TrajDes_GUI', TrajDes_Srv,
                                        self.handle_TrajDes_service)

        # initialize initial time for trajectory generation
        self.time_TrajDes_t0 = rospy.get_time()

        #-----------------------------------------------------------------------#
        # flag for MOCAP is initialized as FALSE
        # flag for wheter Mocap is being used or not
        self.flagMOCAP = False
        # Flag for whether Mocap is ON or OFF
        self.flagMOCAP_On = False
        # Service is created, so that Mocap is turned ON or OFF whenever we want
        Save_MOCAP_service = rospy.Service('Mocap_Set_Id', Mocap_Id,
                                           self.handle_Mocap)

        # Service for providing list of available mocap bodies to GUI
        mocap_available_bodies = rospy.Service('MocapBodies', MocapBodies,
                                               self.handle_available_bodies)

        #-----------------------------------------------------------------------#
        # Service is created, so that user can change controller on GUI
        Chg_Contller = rospy.Service('Controller_GUI', Controller_Srv,
                                     self.handle_Controller_Srv)

        #-----------------------------------------------------------------------#
        # Service for publishing state of controller
        # we use same type of service as above
        Contller_St = rospy.Service('Controller_State_GUI', Controller_Srv,
                                    self.handle_Controller_State_Srv)

        #-----------------------------------------------------------------------#

        self.RotorSObject = RotorSConverter()
        self.RotorSFlag = True

        # subscriber: to odometry
        # self.sub_odometry = rospy.Subscriber("/firefly/odometry_sensor1/odometry", Odometry, self.compute_cmd)
        self.sub_odometry = rospy.Subscriber(
            "/firefly/ground_truth/odometry", Odometry,
            self.get_state_from_rotorS_simulator)

        # # subscriber: to odometry
        # # self.sub_odometry = rospy.Subscriber("/firefly/odometry_sensor1/odometry", Odometry, self.compute_cmd)
        # self.sub_odometry_load = rospy.Subscriber("/firefly/ground_truth/odometry_load", Odometry, self.update_load_odometry)

        # publisher: command firefly motor speeds
        self.pub_motor_speeds = rospy.Publisher('/firefly/command/motor_speed',
                                                Actuators,
                                                queue_size=10)
        #-----------------------------------------------------------------------#

        # rc_override = rospy.Publisher('mavros/rc/override',OverrideRCIn,queue_size=10)
        rc_override = rospy.Publisher('mavros/rc/override',
                                      OverrideRCIn,
                                      queue_size=100)

        pub = rospy.Publisher('mavros/rc/override',
                              OverrideRCIn,
                              queue_size=100)

        rate = rospy.Rate(self.frequency)

        t0 = rospy.get_time()

        self.IrisPlusConverterObject = IrisPlusConverter()

        while not rospy.is_shutdown():

            time = rospy.get_time()

            # get state: if MOCAP is ON we ask MOCAP; if MOCAP in OFF, we are subscribed to Simulator topic
            self.GET_STATE_()

            #print(self.state_quad[0:3])
            # print self.state_quad[6:9]

            # states for desired trajectory
            states_d = self.traj_des()

            # compute input to send to QUAD
            desired_3d_force_quad = self.ControllerObject.output(
                time, self.state_quad, states_d)

            euler_rad = self.state_quad[6:9] * math.pi / 180
            euler_rad_dot = numpy.zeros(3)

            # convert to iris + standard
            state_for_yaw_controller = numpy.concatenate(
                [euler_rad, euler_rad_dot])
            input_for_yaw_controller = numpy.array([0.0, 0.0])
            yaw_rate = self.YawControllerObject.output(
                state_for_yaw_controller, input_for_yaw_controller)

            self.IrisPlusConverterObject.set_rotation_matrix(euler_rad)
            iris_plus_rc_input = self.IrisPlusConverterObject.input_conveter(
                desired_3d_force_quad, yaw_rate)

            # Publish commands to Quad
            self.PublishToQuad(iris_plus_rc_input)

            # publish to GUI (it also contains publish state of Control to GUI)
            self.PublishToGui(states_d, iris_plus_rc_input)

            # ORDER OF INPUTS IS VERY IMPORTANT: roll, pitch, throttle,yaw_rate
            # create message of type OverrideRCIn
            rc_cmd = OverrideRCIn()
            other_channels = numpy.array([1500, 1500, 1500, 1500])
            channels = numpy.concatenate([iris_plus_rc_input, other_channels])
            channels = numpy.array(channels, dtype=numpy.uint16)
            rc_cmd.channels = channels
            rc_override.publish(rc_cmd)

            # publish message
            # TOTAL_MASS = 1.66779; Force3D = numpy.array([0.0,0.0,TOTAL_MASS*9.85]); PsiStar = 0.0; self.pub_motor_speeds.publish(self.RotorSObject.rotor_s_message(Force3D,PsiStar))
            self.pub_motor_speeds.publish(
                self.RotorSObject.rotor_s_message(desired_3d_force_quad,
                                                  yaw_rate))

            if self.SaveDataFlag == True:
                # if we want to save data
                numpy.savetxt(self.file_handle, [
                    concatenate([[rospy.get_time()], [self.flag_measurements],
                                 self.state_quad, states_d[0:9], Input_to_Quad,
                                 self.ControllerObject.d_est])
                ],
                              delimiter=' ')

            # go to sleep
            rate.sleep()
    def control_compute(self):

        # node will be named quad_control (see rqt_graph)
        rospy.init_node('quad_control', anonymous=True)

        # message published by quad_control to GUI 
        self.pub = rospy.Publisher('quad_state_and_cmd', quad_state_and_cmd, queue_size=10)
        
        # message published by quad_control that simulator will subscribe to 
        self.pub_cmd = rospy.Publisher('quad_cmd', quad_cmd, queue_size=10)

        # for publishing state of the controller
        self.pub_ctr_st = rospy.Publisher('ctr_state', Controller_State, queue_size=10)
        # initialize flag for publishing controller state at false
        self.flagPublish_ctr_st = False

        # controller needs to have access to STATE of the system
        # this can come from QUALISYS, a sensor, or the simulator
        self.SubToSim = rospy.Subscriber("quad_state", quad_state, self.get_state) 

        #-----------------------------------------------------------------------#
        #-----------------------------------------------------------------------#
        # TO SAVE DATA FLAG
        # by default, NO data is saved
        self.SaveDataFlag = False
        # we will use this just for convenience, when generating the names of the files
        # where the data will be saved
        self.TimeSaveData = rospy.get_time()
        # Service is created, so that data is saved when GUI requests
        Save_data_service = rospy.Service('SaveDataFromGui', SaveData, self.handle_Save_Data)


        #-----------------------------------------------------------------------#
        #-----------------------------------------------------------------------#
        # SERVICE FOR SELECTING DESIRED TRAJECTORY
        # by default, STAYING STILL IN ORIGIN IS DESIRED TRAJECTORY
        # self.flagTrajDes = 0
        # Service is created, so that data is saved when GUI requests
        TrajDes_service = rospy.Service('TrajDes_GUI', TrajDes_Srv, self.handle_TrajDes_service)

        # initialize initial time for trajectory generation
        self.time_TrajDes_t0 = rospy.get_time()

        #-----------------------------------------------------------------------#
        #-----------------------------------------------------------------------#
        # flag for MOCAP is initialized as FALSE
        # flag for wheter Mocap is being used or not
        self.flagMOCAP    = False
        # Flag for whether Mocap is ON or OFF
        self.flagMOCAP_On = False
        # Service is created, so that Mocap is turned ON or OFF whenever we want
        Save_MOCAP_service = rospy.Service('Mocap_Set_Id', Mocap_Id, self.handle_Mocap)


        # Service for providing list of available mocap bodies to GUI
        mocap_available_bodies = rospy.Service('MocapBodies', MocapBodies, self.handle_available_bodies)


        #-----------------------------------------------------------------------#
        #-----------------------------------------------------------------------#
        # Service is created, so that user can change controller on GUI
        Chg_Contller = rospy.Service('Controller_GUI', Controller_Srv, self.handle_Controller_Srv)


        #-----------------------------------------------------------------------#
        #-----------------------------------------------------------------------#
        # Service for publishing state of controller 
        # we use same type of service as above
        Contller_St = rospy.Service('Controller_State_GUI', Controller_Srv, self.handle_Controller_State_Srv)


        

        # rc_override = rospy.Publisher('mavros/rc/override',OverrideRCIn,queue_size=10)
        rc_override = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=100)

        pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=100)

        rate = rospy.Rate(self.frequency)

        t0 = rospy.get_time() 

        while not rospy.is_shutdown():

            time = rospy.get_time()

            # get state:
            # if MOCAP in on we ask MOCAP
            # if MOCAP in off, we are subscribed to Simulator topic
            self.GET_STATE_()

            # states for desired trajectory
            states_d = self.traj_des()

            # compute input to send to QUAD
            Input_to_Quad = self.controller.output(time,self.state_quad,states_d)
            
            # Publish commands to Quad
            self.PublishToQuad(Input_to_Quad)

            # publish to GUI (it also contains publish state of Control to GUI)
            self.PublishToGui(states_d,Input_to_Quad)


            # ORDER OF INPUTS IS VERY IMPORTANT
            # roll,pitch,throttle,yaw_rate
            # create message of type OverrideRCIn
            rc_cmd          = OverrideRCIn()
            other_channels  = numpy.array([1500,1500,1500,1500])
            aux             = numpy.concatenate([Input_to_Quad,other_channels])
            aux             = numpy.array(aux,dtype=numpy.uint16)
            rc_cmd.channels = aux
            # rc_cmd.channels = numpy.array(rc_cmd.channels,dtype=numpy.uint16)
            # rospy.logwarn(rc_cmd.channels)
            # rospy.logwarn(aux)
            rc_override.publish(rc_cmd)


            if self.SaveDataFlag == True:
                # if we want to save data
                numpy.savetxt(self.file_handle, [concatenate([[rospy.get_time()],[self.flag_measurements], self.state_quad, states_d[0:9], Input_to_Quad,self.controller.d_est])],delimiter=' ')                    

            # go to sleep
            rate.sleep()    
Exemple #14
0
    def update_controller(self):
        if not self.is_localization_ready or not self.is_target_ready:
            # stop motors
            self.RCOR_msg = OverrideRCIn()
            self.RCOR_msg.channels = [
                1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500
            ]
            self.mavros_vel_pub.publish(self.RCOR_msg)
            return

        distance_x = self.current_target[0] - self.robot_position_x
        distance_y = self.current_target[1] - self.robot_position_y
        distance_z = self.current_target[2] - self.robot_position_z

        desired_yaw = math.atan2(distance_y, distance_x)

        if desired_yaw >= self.robot_yaw:
            error_orientation = desired_yaw - self.robot_yaw
        else:
            error_orientation = self.robot_yaw - desired_yaw

        error_orientation = (error_orientation % 2 *
                             math.pi) * self.rad2degrees

        error_position = math.sqrt(
            math.pow(distance_x, 2.0) + math.pow(distance_y, 2.0))

        error_depth = distance_z

        self.position_pid.SetPoint = 0.0
        self.position_pid.update(error_position)

        self.orientation_pid.SetPoint = desired_yaw
        self.orientation_pid.update(self.robot_yaw)
        self.orientation_pid.output = self.bound_limit(
            self.orientation_pid.output, -1000, 1000)

        self.depth_pid.SetPoint = self.current_target[2]
        self.depth_pid.update(self.robot_position_z)

        if abs(error_orientation) >= self.threshold_orientation or \
            error_position >= self.threshold_position or \
            error_depth >= self.threshold_depth:

            K_ORI = 1500 + self.orientation_pid.output
            K_ORI = self.bound_limit(K_ORI, 1100, 1900)

            K_POS = 1500 - self.position_pid.output
            K_POS = self.bound_limit(K_POS, 1100, 1900)

            K_DEP = 1500 - self.depth_pid.output
            K_DEP = self.bound_limit(K_DEP, 1100, 1900)

            # set speed thrusters
            self.RCOR_msg = OverrideRCIn()
            self.RCOR_msg.channels = [
                1500, 1500, 1500, K_ORI, K_POS, K_DEP, 1500, 1500
            ]
            self.mavros_vel_pub.publish(self.RCOR_msg)

        else:
            self.waypointReached()
Exemple #15
0
# 3D point & Stamped Pose msgs & Orientation as quaternion
from geometry_msgs.msg import Point, PoseStamped, Quaternion, Twist
#import math for arctan and sqrt function
from math import atan2, sqrt, pi, cos, sin
#import quaternion transformation
from tf.transformations import euler_from_quaternion
import numpy as np
from std_msgs.msg import String
from mavros_msgs.msg import OverrideRCIn,RCIn
import scipy.io
import time
import os



RcOver                  = OverrideRCIn()
RcOver.channels = [1500,1500,1500,1500,0,0,0,0]
#M       = np.empty(1)
#alpha   = np.empty(1)
#beta    = np.empty(1)
prev_s  = 0
prev_v  = 0
x       = 0
y       = 0
lx       = 0
ly       = 0
vdot       = 0
wdot       = 0
v   	= 0
w 	= 0
lv 	=0
Exemple #16
0
    def start(self):

        arefa = 20
        center_point = (0, 0)

        for i in range(10):
            #  解除锁定
            self.arm_state = self.arm()
            self.manual_state = self.manual()
            time.sleep(0.2)
        r = rospy.Rate(10)  # 设置数据发送频率为10hz
        rcmsg = OverrideRCIn()
        rcmsg.channels = [
            65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535
        ]
        # 定义油门
        # rcmsg.channels[2] = 1500

        # 启动获取距离
        # Thread(target=self.get_dist, args=()).start()
        # 设置pid
        # Thread(target=self.set_pid,args=()).start()
        first_cortron = True
        while self.arm_state and self.manual_state and (rospy.is_shutdown() is
                                                        False):

            # print("1")
            #
            if len(self.pointsClassification[3]) >= 0 and self.rc_in[5] > 1500:
                first_cortron = True
                # 清除原有图像
                start_time = time.time()
                cen = CenterPointEdge.CenterPointEdge(
                    self.pointsClassification, center_point, 1)
                paths = cen.contour()

                # print(paths)
                # 计算单折线斜率
                slope, intercept, r_value, p_value, std_err = st.linregress(
                    paths)

                # 当线段拟合程度不够时使用双折线拟合
                if r_value**2 < 0.75:
                    lines = LineSegment.segment2(paths,
                                                 center_point=center_point,
                                                 debug=False,
                                                 debug_data=paths)
                    # 双折线中选择k 值为负值且拟合程度大于0.75的线
                    # 次选拟合程度最大的
                    # print (lines    )
                    if (lines[0].slope > 0 and lines[0].r_value > 0.75):
                        slope = lines[0].slope
                        intercept = lines[0].intercept
                    elif lines[0].r_value < 0.75 and lines[1].r_value == 1.0:
                        slope = float("inf")
                        intercept = 0
                    else:
                        slope = lines[1].slope
                        intercept = lines[1].intercept

                self.plot(paths, slope, intercept)

                # 线性拟合,可以返回斜率,截距,r 值,p 值,标准误差
                # slope*x - y + intercept = 0
                x = []
                for path in paths:
                    x.append(path[0])

                if slope == float("inf"):
                    deflection = 0
                    # 平均x 值当做距离
                    left_dist = abs(list(np.average(paths, axis=0))[0])
                else:

                    deflection = math.atan(slope) * 180.0 / math.pi
                    if deflection > 0:
                        deflection -= 90
                    else:
                        deflection += 90
                    # 船到岸边的距离
                    left_dist = abs(center_point[0] * slope - center_point[0] +
                                    intercept) / math.sqrt(slope**2 + 1)
                # 均值滤波
                self.left_dist = self.avg_filter(self.left_dist_queue,
                                                 left_dist, self.filter_N)
                # print ("defle",deflection)
                # 岸线的平行的角度
                left_parallel_yaw = (self.yaw - deflection) % 360
                # 均值滤波
                self.left_parallel_yaw = self.avg_filter(
                    self.left_parallel_yaw_queue, left_parallel_yaw,
                    self.filter_N)

                # print ("left_parallel_yaw: ",self.left_parallel_yaw  )
                end_time = time.time()
                # print ("time1: ", end_time-start_time)
                if self.left_dist != None and self.left_parallel_yaw != None:
                    # 根据与岸的距离不断调整 yaw 是其不断逼近想要的距离
                    self.set_dist_pid_control(self.except_dist, self.left_dist,
                                              self.left_parallel_yaw, rcmsg)
                    # 调试直行程序pid
                    # self.set_yaw_pid_control(311,rcmsg)
                    # print ("rcmsg: ",rcmsg)
                    # self.print_log()  # 打印日志
                    self.rc_override_pub.publish(rcmsg)
                # time.sleep(0.1)
            else:
                # rcmsg.channels = [65535, 65535, 65535,
                #                 65535, 65535, 65535, 65535, 65535]
                if first_cortron:
                    rcmsg.channels[0] = 1500
                    first_cortron = False
                else:
                    rcmsg.channels[0] = 65535

                # print ("rcmsg: ",rcmsg)
                # print ("self.rc_in", self.rc_in)
                # self.print_log()  # 打印日志
                self.rc_override_pub.publish(rcmsg)
                time.sleep(0.1)
Exemple #17
0
    def send_rc_command(self):
        if not self.is_manual:
            return

        if self.state == 'done' or self.state == 'finished':
            return

        with self.nodeLock:
            avoid = self.avoid_direction
            speed = self.arduino_speed_value
            last_touch = self.last_touch
            last_rc3 = self.last_rc3_raw
            enc = self.encoder_distance
            self.updates = 0

        if self.state == None:
            self.state = 'neutral_1'
            self.state_start = datetime.now()
            self.throttle_pid.reset_i()
            self.last_throttle = 0

        diff = (datetime.now() - self.state_start).total_seconds()
        if diff >= self.states[self.state]['max_time']:
            if self.advance_state():
                return

        try:
            self.touchhandler_pub.publish(True)
        except:
            rospy.logerr("touchhandler_pub() exception")

        if 'target_distance' in self.states[self.state].keys():
            if self.state_target_distance == None:
                self.state_target_distance = enc

            d = enc - self.state_target_distance
            td = self.states[self.state]['target_distance']
            if (td < 0 and d <= td) or (td > 0 and d >= td):
                if self.advance_state():
                    return

        if 'target_speed' in self.states[self.state].keys():
            if abs(speed - self.states[self.state]['target_speed']
                   ) <= 0.2 and self.state_target_speed_reached == None:
                self.state_target_speed_reached = datetime.now()

            if self.state_target_speed_reached != None:
                diff = (datetime.now() -
                        self.state_target_speed_reached).total_seconds()
                if diff >= self.states[self.state]['target_time']:
                    if self.advance_state():
                        return

        orc = OverrideRCIn()
        orc.channels[1] = 0
        orc.channels[3] = 0
        orc.channels[4] = 0
        orc.channels[5] = 0
        orc.channels[6] = 0
        orc.channels[7] = 0

        orc.channels[0] = self.apm_rc1_trim  #  straight

        if 'steer_offset' in self.states[self.state].keys():
            o = self.states[self.state]['steer_offset']
            reverse_mul = 1
            if self.apm_rc1_reversed != 0:
                reverse_mul = -1
            o *= reverse_mul

            if avoid > 0:
                if avoid == 1:
                    orc.channels[0] -= o
                else:
                    orc.channels[0] += o
            else:
                if last_touch < 2:
                    orc.channels[0] -= o
                else:
                    orc.channels[0] += o

        orc.channels[2] = self.apm_rc3_trim  # neutral

        if 'target_speed' in self.states[self.state].keys():
            if 'throttle_offset' in self.states[self.state].keys():
                orc.channels[2] += self.states[self.state]['throttle_offset']
            else:
                if self.last_throttle == 0:
                    if last_rc3 - self.apm_rc3_trim < 0:
                        self.last_throttle = float(
                            last_rc3 - self.apm_rc3_trim) / float(
                                self.apm_rc3_trim - self.apm_rc3_min) * 1000.0
                    else:
                        self.last_throttle = float(
                            last_rc3 - self.apm_rc3_trim) / float(
                                self.apm_rc3_max - self.apm_rc3_trim) * 1000.0

                last_t = self.last_throttle
                target_speed = self.states[self.state]['target_speed']
                error = target_speed - speed

                last_t += self.throttle_pid.get_pid(error, 7.5)
                last_t = self.constrain(
                    last_t, self.states[self.state]['throttle_min'],
                    self.states[self.state]['throttle_max'])  # 0.1 of percents

                if last_t < 0.0:
                    orc.channels[2] += int(
                        last_t / 1000.0 *
                        float(self.apm_rc3_trim - self.apm_rc3_min))
                else:
                    orc.channels[2] += int(
                        last_t / 1000.0 *
                        float(self.apm_rc3_max - self.apm_rc3_trim))

                self.last_throttle = last_t

        rospy.loginfo("Send rc: %d %d" % (orc.channels[0], orc.channels[2]))
        self.rc_override_pub.publish(orc)
Exemple #18
0
    def onMessage(self, payload, isBinary):
        # Debug
        if isBinary:
            print("Binary message received: {0} bytes".format(len(payload)))
        else:
            print("Text message received: {0}".format(payload.decode('utf8')))

            # Do stuff
            # pub = rospy.Publisher('blockly', String, queue_size=10)
            # time.sleep(1)
            # pub.publish("blockly says: "+payload.decode('utf8'))

            # Simple text protocol for communication
            # first line is the name of the method
            # next lines are body of message
            message_text = payload.decode('utf8')
            message_data = message_text.split('\n', 1)

            if len(message_data) > 0:
                method_name = message_data[0]
                if len(message_data) > 1:
                    method_body = message_data[1]
                    if method_name.startswith('play'):
                        CodeStatus.set_current_status(CodeStatus.RUNNING)
                        BlocklyServerProtocol.build_ros_node(method_body)
                        rospy.loginfo('The file generated contains...')
                        os.system('cat test.py')
                        if method_name == 'play2':
                            CodeExecution.run_process(['python', 'test.py'])
                        elif method_name == 'play3':
                            CodeExecution.run_process(['python3', 'test.py'])
                    else:
                        rospy.logerr('Called unknown method %s', method_name)
                else:
                    if 'pause' == method_name:
                        CodeStatus.set_current_status(CodeStatus.PAUSED)
                    elif 'resume' == method_name:
                        CodeStatus.set_current_status(CodeStatus.RUNNING)
                    elif 'end' == method_name:
                        #End test.py execution
                        global pid
                        print("@@@@@@@@@@@@@@@@@@")
                        try:
                            print("kill pid="+str(pid))
                            os.kill(pid, signal.SIGKILL)
                            ros_nodes = rosnode.get_node_names()
                        except NameError:
                            print("execution script not running.")
                            pass

                        if '/imu_talker' in ros_nodes: #brain
                            ##set default values
                            pub = rospy.Publisher('/statusleds', String, queue_size=10, latch=True)
                            msg = 'blue_off'
                            pub.publish(msg)
                            msg = 'orange_off'
                            pub.publish(msg)
                        if '/crab_leg_kinematics' in ros_nodes: #spider
                            print("spider running")
                            pub = rospy.Publisher('/joy', Joy, queue_size=10, latch=True)
                            msg = Joy()
                            msg.header.stamp = rospy.Time.now()
                            rate = rospy.Rate(10)
                            valueAxe = 0.0
                            valueButton = 0
                            for i in range (0, 20):
                                msg.axes.append(valueAxe)
                            for e in range (0, 17):
                                msg.buttons.append(valueButton)
                            pub.publish(msg)
                            print("DEFAULT MESSAGES SENT")
                        if '/mavros' in ros_nodes: #rover
                            print("rover")
                            pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
                            msg = OverrideRCIn()
                            msg.channels[0] = 1500
                            msg.channels[1] = 0
                            msg.channels[2] = 1500
                            msg.channels[3] = 0
                            msg.channels[4] = 0
                            msg.channels[5] = 0
                            msg.channels[6] = 0
                            msg.channels[7] = 0
                            pub.publish(msg)
                        print("@@@@@@@@@@@@@@@@@@")

                    elif method_name.startswith('control'):
                        robot = method_name.split('control_')[1]
                        if robot.startswith('spider'):
                            direction = robot.split('spider_')[1]
                            pub = rospy.Publisher('/joy', Joy, queue_size=10)
                            msg = Joy()
                            msg.header.stamp = rospy.Time.now()
                            rate = rospy.Rate(10)
                               
                            valueAxe = 0.0
                            valueButton = 0
                            for i in range (0, 20):
                                msg.axes.append(valueAxe)
                            for e in range (0, 17):
                                msg.buttons.append(valueButton)

                            if direction == 'up':#forward
                                msg.axes[1] = 1
                            elif direction == 'down':#backwards
                                msg.axes[1] = -1
                            elif direction == 'left':#turn left
                                #msg.axes[0] = 1
                                msg.axes[2] = 1
                            elif direction == 'right':#turn rigth
                                #msg.axes[0] = -1
                                msg.axes[2] = -1

                            pub.publish(msg)
                            rate.sleep()

                        elif robot.startswith('rover'):
                            direction = robot.split('rover_')[1]   
                            rospy.wait_for_service('/mavros/set_mode')
                            change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode)
                            resp1 = change_mode(custom_mode='manual')
                            print (resp1)
                            if 'True' in str(resp1):
                                pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
                                r = rospy.Rate(10) #2hz
                                msg = OverrideRCIn()
                                throttle_channel=2
                                steer_channel=0

                                speed_slow = 1558
                                #speed_turbo = 2000 #dangerous
                                speed = speed_slow

                                if direction == 'up':#forward
                                    msg.channels[throttle_channel]=speed
                                    msg.channels[steer_channel]=1385 #straight
                                elif direction == 'down':#backwards
                                    msg.channels[throttle_channel]=1450 #slow
                                    msg.channels[steer_channel]=1385 #straight
                                elif direction == 'left':#turn left
                                    msg.channels[throttle_channel]=speed
                                    msg.channels[steer_channel]=1285
                                elif direction == 'right':#turn rigth
                                    msg.channels[throttle_channel]=speed
                                    msg.channels[steer_channel]=1485

                                start = time.time()
                                flag=True
                                while not rospy.is_shutdown() and flag:
                                    sample_time=time.time()
                                    if ((sample_time - start) > 0.5):
                                        flag=False
                                    pub.publish(msg)
                                    r.sleep()
                    else:
                        rospy.logerr('Called unknown method %s', method_name)
Exemple #19
0
    def __init__(self, current_target, debug=False):
        self.degrees2rad = math.pi / 180.0
        self.rad2degrees = 180.0 / math.pi
        self.debug = debug
        self.current_target = current_target
        self.list_points = []

        # define rate of  10 hz
        self.rate = rospy.Rate(50.0)

        # init variables for odometry
        [self.robot_position_x, self.robot_position_y,
         self.robot_position_z] = [0, 0, 0]
        [self.robot_roll, self.robot_pitch, self.robot_yaw] = [0, 0, 0]
        """initialize controllers"""
        self.threshold_orientation = 0.0

        self.S_kp = 0.0
        self.S_kd = 0.0
        self.S_ki = 0.0
        self.S_windup = 0.0
        self.controller_pid = PID()
        self.controller_pid.SetPoint = 0.0
        self.controller_pid.setSampleTime(0.01)
        self.controller_pid.setKp(self.S_kp)
        self.controller_pid.setKd(self.S_kd)
        self.controller_pid.setKi(self.S_ki)
        self.controller_pid.setWindup(self.S_windup)

        # event publisher
        self.event_pub = rospy.Publisher("/robdos/stateEvents",
                                         StateEvent,
                                         queue_size=1)
        self.event_msg = StateEvent()

        # mavros velocity publisher
        self.mavros_vel_pub = rospy.Publisher("/mavros/rc/override",
                                              OverrideRCIn,
                                              queue_size=1)
        self.RCOR_msg = OverrideRCIn()

        ######################################################SUBSCRIBERS########################################################
        # create subscriber for robot localization
        # self.sub_localization = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.process_localization_message, queue_size=1)
        self.sub_localization = rospy.Subscriber(
            '/robdos/odom',
            Odometry,
            self.process_localization_message,
            queue_size=1)
        # self.sub_localization = rospy.Subscriber('/mavros/global_position/local', Odometry, self.process_localization_message, queue_size=1)

        self.sub_waypoint_list = rospy.Subscriber(
            '/mavros/mission/waypoints',
            WaypointList,
            self.process_waypoint_message,
            queue_size=1)

        # reconfigure service
        self.srv = Server(
            controllerOrientationConfig,
            self.reconfig_callback)  # define dynamic_reconfigure callback
Exemple #20
0
%%%%%%%%%%%%%%%%%%%%%%%
command_cotrol
%%%%%%%%%%%%%%%%%%%%%%%
0 : ARM
1 : TAKEOFF
2 : OFFBOARDS
3 : LAND
4 : POSCTL
5 : ATTITCTL
---------------------------

CTRL-C to quit

"""
speed_control = 1600
cur_target_rc_yaw = OverrideRCIn()
mavros_state = State()
armServer = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
setModeServer = rospy.ServiceProxy('/mavros/set_mode', SetMode)
local_target_pub = rospy.Publisher('/mavros/rc/override',
                                   OverrideRCIn,
                                   queue_size=100)


def __init__():
    rospy.init_node('PX4_control', anonymous=True)
    rospy.Subscriber("/mavros/state", State, mavros_state_callback)
    print("Initialized")


def RCInOverride(channel0, channel1, channel2, channel3):
Exemple #21
0
        mode = "GUIDED"
        print "Mode " + mode
        setMode(mode)
    elif data.buttons[8]:
        mode = "ALT_HOLD"
        print "Mode " + mode
        setMode(mode)
    elif data.buttons[9]:
        mode = "CIRCLE"
        print "Mode " + mode
        setMode(mode)
    elif data.buttons[10]:
        mode = "AUTO"
        print "Mode " + mode
        setMode(mode)
    elif data.buttons[11]:
        mode = "ACRO"
        print "Mode " + mode
        setMode(mode)


rospy.init_node("DRONE_CTRL")
cmd_vel_pub = rospy.Publisher("/mavros/rc/override",
                              OverrideRCIn,
                              queue_size=10)
rospy.Subscriber("/joy", Joy, callback)
cmd = OverrideRCIn()

while not rospy.is_shutdown():
    cmd_vel_pub.publish(cmd)
    time.sleep(0.01)
    def control_compute(self):

        # node will be named quad_control (see rqt_graph)
        rospy.init_node('quad_control', anonymous=True)

        # message published by quad_control to GUI 
        self.pub = rospy.Publisher('quad_state_and_cmd', quad_state_and_cmd, queue_size=10)
        
        # message published by quad_control that simulator will subscribe to 
        self.pub_cmd = rospy.Publisher('quad_cmd', quad_cmd, queue_size=10)

        # for publishing state of the controller
        self.pub_ctr_st = rospy.Publisher('ctr_state', Controller_State, queue_size=10)
        # initialize flag for publishing controller state at false
        self.flagPublish_ctr_st = False

        # controller needs to have access to STATE of the system
        # this can come from QUALISYS, a sensor, or the simulator
        self.SubToSim = rospy.Subscriber("quad_state", quad_state, self.get_state_from_simulator) 

        #-----------------------------------------------------------------------#
        # TO SAVE DATA FLAG
        # by default, NO data is saved
        self.SaveDataFlag = False
        # we will use this just for convenience, when generating the names of the files
        # where the data will be saved
        self.TimeSaveData = rospy.get_time()
        # Service is created, so that data is saved when GUI requests
        Save_data_service = rospy.Service('SaveDataFromGui', SaveData, self._handle_save_data)


        #-----------------------------------------------------------------------#
        # SERVICE FOR SELECTING DESIRED TRAJECTORY
        # by default, STAYING STILL IN ORIGIN IS DESIRED TRAJECTORY
        # self.flagTrajDes = 0
        # Service is created, so that data is saved when GUI requests
        TrajDes_service = rospy.Service('ServiceTrajectoryDesired', SrvTrajectoryDesired, self._handle_service_trajectory_des)

        # initialize initial time for trajectory generation
        self.time_TrajDes_t0 = rospy.get_time()

        #-----------------------------------------------------------------------#
        # flag for MOCAP is initialized as FALSE
        # flag for wheter Mocap is being used or not
        self.flagMOCAP    = False
        # Flag for whether Mocap is ON or OFF
        self.flagMOCAP_On = False
        # Service is created, so that Mocap is turned ON or OFF whenever we want
        Save_MOCAP_service = rospy.Service('Mocap_Set_Id', Mocap_Id, self.handle_Mocap)

        # Service for providing list of available mocap bodies to GUI
        mocap_available_bodies = rospy.Service('MocapBodies', MocapBodies, self.handle_available_bodies)

        #-----------------------------------------------------------------------#
        # Service is created, so that user can change controller on GUI
        rospy.Service('ServiceChangeController', SrvControllerChangeByStr, self._handle_service_change_controller)
        # rospy.Service('ServiceChangeController', SrvControllerChange, self._handle_service_change_controller)


        #-----------------------------------------------------------------------#
        # Service: change neutral value that guarantees that a quad remains at a desired altitude
        rospy.Service('IrisPlusResetNeutral', IrisPlusResetNeutral, self._handle_iris_plus_reset_neutral)
        rospy.Service('IrisPlusSetNeutral', IrisPlusSetNeutral, self._handle_iris_plus_set_neutral)

        #-----------------------------------------------------------------------#
        # Service for publishing state of controller 
        # we use same type of service as above
        #Contller_St = rospy.Service('Controller_State_GUI', Controller_Srv, self.handle_Controller_State_Srv)

        #-----------------------------------------------------------------------#

        self.RotorSObject = RotorSConverter()
        self.RotorSFlag   = True


        # subscriber: to odometry
        # self.sub_odometry = rospy.Subscriber("/firefly/odometry_sensor1/odometry", Odometry, self.compute_cmd)
        self.sub_odometry = rospy.Subscriber("/firefly/ground_truth/odometry", Odometry, self.get_state_from_rotorS_simulator) 

        # # subscriber: to odometry
        # # self.sub_odometry = rospy.Subscriber("/firefly/odometry_sensor1/odometry", Odometry, self.compute_cmd)
        # self.sub_odometry_load = rospy.Subscriber("/firefly/ground_truth/odometry_load", Odometry, self.update_load_odometry) 

        # publisher: command firefly motor speeds 
        self.pub_motor_speeds = rospy.Publisher('/firefly/command/motor_speed', Actuators, queue_size=10)
        #-----------------------------------------------------------------------#
        
        # rc_override = rospy.Publisher('mavros/rc/override',OverrideRCIn,queue_size=10)
        rc_override = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=100)

        pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=100)

        rate = rospy.Rate(self.frequency)

        t0 = rospy.get_time() 

        self.IrisPlusConverterObject = IrisPlusConverter()

        while not rospy.is_shutdown():

            time = rospy.get_time()

            # get state: if MOCAP is ON we ask MOCAP; if MOCAP in OFF, we are subscribed to Simulator topic
            self.GET_STATE_()

            #print(self.state_quad[0:3])
            # print self.state_quad[6:9]

            # states for desired trajectory
            states_d = self.traj_des()

            # compute input to send to QUAD
            # rospy.logwarn(self.ControllerObject.__class__.__name__)
            # rospy.logwarn(self.state_quad[0:3])
            # rospy.logwarn('bbbbbbbbbbbbbbbbbbbbbb')
            # rospy.logwarn(states_d[0:3])
            # rospy.logwarn('ccccccccccccccccccccccccccc')
            desired_3d_force_quad = self.ControllerObject.output(time,self.state_quad,states_d)
            # rospy.logwarn('dddddddddddddd')
            # rospy.logwarn(desired_3d_force_quad)
            self.DesiredZForceMedian.update_data(desired_3d_force_quad[2])


            euler_rad     = self.state_quad[6:9]*math.pi/180
            euler_rad_dot = numpy.zeros(3)

            # convert to iris + standard
            state_for_yaw_controller = numpy.concatenate([euler_rad,euler_rad_dot])
            input_for_yaw_controller = numpy.array([0.0,0.0])
            yaw_rate = self.YawControllerObject.output(state_for_yaw_controller,input_for_yaw_controller)
            
            self.IrisPlusConverterObject.set_rotation_matrix(euler_rad)
            iris_plus_rc_input = self.IrisPlusConverterObject.input_conveter(desired_3d_force_quad,yaw_rate)


            # Publish commands to Quad
            self.PublishToQuad(iris_plus_rc_input)

            # publish to GUI (it also contains publish state of Control to GUI)
            self.PublishToGui(states_d,iris_plus_rc_input)


            # ORDER OF INPUTS IS VERY IMPORTANT: roll, pitch, throttle,yaw_rate
            # create message of type OverrideRCIn
            rc_cmd          = OverrideRCIn()
            other_channels  = numpy.array([1500,1500,1500,1500])
            channels        = numpy.concatenate([iris_plus_rc_input,other_channels])
            channels        = numpy.array(channels,dtype=numpy.uint16)
            rc_cmd.channels = channels
            rc_override.publish(rc_cmd)

            # publish message
            # TOTAL_MASS = 1.66779; Force3D = numpy.array([0.0,0.0,TOTAL_MASS*9.85]); PsiStar = 0.0; self.pub_motor_speeds.publish(self.RotorSObject.rotor_s_message(Force3D,PsiStar))
            self.pub_motor_speeds.publish(self.RotorSObject.rotor_s_message(desired_3d_force_quad,yaw_rate))


            if self.SaveDataFlag == True:
                # if we want to save data
                # numpy.savetxt(self.file_handle, [concatenate([[rospy.get_time()],[self.flag_measurements], self.state_quad, states_d[0:9], Input_to_Quad,self.ControllerObject.d_est])],delimiter=' ')
                numpy.savetxt(self.file_handle, [concatenate([[rospy.get_time()],[self.flag_measurements], self.state_quad, states_d[0:9], desired_3d_force_quad])],delimiter=' ')
            # go to sleep
            rate.sleep() 
Exemple #23
0
    def _step(self, action):

        msg = OverrideRCIn()

        if action == 0:  #FORWARD
            msg.channels[0] = 1500  # Yaw
            msg.channels[2] = 1900  # Throttle
        elif action == 1:  #LEFT
            msg.channels[0] = 1100  # Yaw
            msg.channels[2] = 1900  # Throttle
        elif action == 2:  #RIGHT
            msg.channels[0] = 1900  # Yaw
            msg.channels[2] = 1900  # Throttle

        msg.channels[1] = 0
        msg.channels[3] = 0
        msg.channels[4] = 0
        msg.channels[5] = 0
        msg.channels[6] = 0
        msg.channels[7] = 0

        self.pub.publish(msg)

        #read laser data
        data = None
        while data is None:
            try:
                data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
            except:
                pass

        #simplify ranges - discretize
        discretized_ranges = []
        discretized_ranges_amount = 5
        min_range = 1.5  #collision

        done = False

        mod = (len(data.ranges) / discretized_ranges_amount)
        for i, item in enumerate(data.ranges):
            if (i % mod == 0):
                if data.ranges[i] == float('Inf'):
                    discretized_ranges.append(int(data.range_max))
                elif np.isnan(data.ranges[i]):
                    discretized_ranges.append(0)
                else:
                    discretized_ranges.append(int(data.ranges[i]))
                    #discretized_ranges.append(round(data.ranges[i] * 2) / 2)
            if (min_range > data.ranges[i] > 0):
                done = True
                #break

        if not done:
            if action == 0:  # FORWARD
                reward = 5
            else:
                reward = 1
        else:
            reward = -200

        state = discretized_ranges

        return state, reward, done, {}