def sendViaMavlink(data):
throttle = data.linear.x
#x direction of linear velocity
yaw = data.angular.z
#rotation around z. Hopefully also a velocity, and not a setpoint...
# next, we'll publish the actuatorControl message over ROS
command = OverrideRCIn()
if not on:
command.channels = [1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500]
#Make stationary
command.channels = [
65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535
]
#Relenquish control
elif rc and not isSetRc:
isSetRc = True
command.channels = [
65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535
]
else:
isSetRc = False
command.channels = [
1500 + 500 * -yaw, 1500, 1500 + 500 * throttle, 1500, 1500, 1500,
1500, 1500
]
# index 0 is yaw, index 2 is throttle.
# publish the message
p.publish(command)
rospy.loginfo("dyaw: " + str(yaw) + ", dx: " + str(throttle))
def set_force(self, event):
msg_actu = OverrideRCIn()
if self.mode==0:
msg_actu.channels = np.array([self.pitch,self.roll,self.yaw,self.z,0,0,0,0])
elif self.mode==1:
msg_actu.channels = np.array([self.z,self.z,self.z,self.z,0,0,0,0])
rospy.loginfo(msg_actu.channels[0:4])
self.pub_actuators.publish(msg_actu.channels)
self.pitch_1 = self.pitch
self.roll_1 = self.roll
self.yaw_1 = self.yaw
self.z_1 = self.z
def service_callback(self, req):
# dictionary to store the index of channel to control the vehicle
# as indicated on the ArduSub website
index = {
'l': 5,
'r': 5,
'f': 4,
'b': 4,
'rl': 3,
'rr': 3,
'u': 2,
'd': 2,
's': -1,
'pu': 0,
'pd': 0
}
# dictionary for easy access to the speeds values
speed = {
'l': self.left_sway_speed,
'r': self.right_sway_speed,
'f': self.forward_speed,
'b': self.backward_speed,
'rl': self.left_rotation_speed,
'rr': self.right_rotation_speed,
'u': self.up_speed,
'd': self.down_speed,
'pu': self.pitch_up_speed,
'pd': self.pitch_down_speed
}
loginfo_msg = {
'l': "lateral left",
'r': "lateral right",
'f': "forward",
'b': "backward",
'rl': "left roatation",
'rr': "right rotation",
'u': "depth up",
'd': "depth down",
'pu': "pitching up",
'pd': "pitching down"
}
self.update_params()
override_msg = OverrideRCIn() #object of the msg type for the RC
if req.direction in index: #error catcher to verify a valid direction is sent
override_msg.channels = [
1500 for x in range(len(override_msg.channels))
]
if req.direction == 's':
self.control_msg_pub.publish(override_msg)
self.rate.sleep()
rospy.loginfo("vehicle stopped")
return True
override_msg.channels[index[req.direction]] = speed[req.direction]
self.control_msg_pub.publish(override_msg)
self.rate.sleep()
rospy.loginfo("The vehicle started doing a %s",
loginfo_msg[req.direction])
return True
else: #catching the error
rospy.logerr("you have endered wrong direction for move service")
def get_control(chrs, frame_id):
debugging = False
msg = OverrideRCIn()
# msg.header.frame_id = "manual_control_frame"
# msg.header.seq = frame_id
#now = rospy.get_rostime()
# msg.header.stamp.secs = now.secs
# msg.header.stamp.nsecs = now.nsecs
#msg.rssi = 0
msg.channels = [1500, 1500, 1100, 1500, 1100, 1100, 1900, 1900]
if 'w' in chrs and 's' not in chrs:
msg.channels[1] = 1575
if 's' in chrs and 'w' not in chrs:
msg.channels[1] = 1425
if 'a' in chrs and 'd' not in chrs:
msg.channels[0] = 1100
if 'd' in chrs and 'a' not in chrs:
msg.channels[0] = 1900
return 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)
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)
def pub_force(self):
rospy.loginfo("Thrusters :=\n %s" % self.thrusters)
msg_actuators = OverrideRCIn()
msg_actuators.channels = np.array([
mur_common.push_to_pwm(self.thrusters[0]),
mur_common.push_to_pwm(self.thrusters[1]),
mur_common.push_to_pwm(self.thrusters[2]),
mur_common.push_to_pwm(self.thrusters[3]), 0, 0, 0, 0
])
rospy.loginfo("Motors Values:= %s",
msg_actuators.channels[0:self.num_thrusters])
self.pub_actuators.publish(msg_actuators)
def main():
override_pub = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=10)
rospy.init_node("mavtest")
mavros.set_namespace("/mavros")
rc = OverrideRCIn()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
rc.channels = [1000,1000,1000,1000,1000,1000,1000,1000]
rospy.loginfo(rc)
override_pub.publish(rc)
rate.sleep()
def MURStopMotors():
pub_actuators = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=5)
rospy.init_node('mur_stop_motors', anonymous=True)
rate = rospy.Rate(10) # 10hz
epochs = 5
for i in range(epochs):
msg_actuators = OverrideRCIn()
msg_actuators.channels = np.array([1500, 1500, 1500, 1500, 0, 0, 0, 0])
rospy.loginfo("Motors Values:= %s", msg_actuators.channels[0:4])
pub_actuators.publish(msg_actuators)
rate.sleep()
rospy.logwarn("The motors have been stopped by themselves.")
def seek_cone(loc):
# Sort the poses by y distance to get the nearest cone
poses = sorted(loc.poses, key=lambda loc: loc.y)
cone_loc = poses[0]
steering = steering_limits[1]
# Steer if not in front
if(cone_loc.x < -20 or cone_loc.x > 20):
# Sin(theta)
z = math.sqrt(cone_loc.x*cone_loc.x + cone_loc.y*cone_loc.y)
#steering = steering + args.steering_factor*500*cone_loc.x/z
steering = steering + args.steering_factor*2*cone_loc.x
# Slowest approach to cone
throttle = throttle_limits[1] + 20
# Use real depth when available for throttle
if(cone_loc.z > 0):
# Real depth is in mm and maximum would probably be less than 6m
if(cone_loc.z > 300):
throttle = throttle + args.throttle_factor*(cone_loc.z - 300)/20
else:
y = cone_loc.y - 40
if(y > 0):
throttle = throttle + args.throttle_factor*(y)
#-- test with fixed throttle to start
throttle = 1675
# Everything must be bounded
if(steering > steering_limits[2]):
steering = steering_limits[2]
if(steering < steering_limits[0]):
steering = steering_limits[0]
if(throttle > throttle_limits[2]):
throttle = throttle_limits[2]
if(throttle < throttle_limits[0]):
throttle = throttle_limits[0]
rc = OverrideRCIn()
rc.channels = [int(steering), 0, int(throttle), 0, 0, 0, 0, 0]
pub.publish(rc)
def __init__(self):
self.pub_sp = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=10)
rospy.init_node('rcOverride', anonymous=True)
rate = rospy.Rate(60) # 10hz
act = OverrideRCIn()
rospy.wait_for_service('mavros/set_mode')
change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
result_mode = change_mode(0, "MANUAL")
print("Sending PWM Override..")
while not rospy.is_shutdown():
act.channels = [1800, 1800, 1800, 1800, 1500, 1500, 0, 0]
#rospy.loginfo(str(act.controls))
self.pub_sp.publish(act)
#result_mode = change_mode(0,"MANUAL")
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_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()
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()
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
def start(self, debug=False):
arefa = 10
center_point = (0, 0)
# for i in range(10):
# # 解除锁定
# self.arm_state = self.arm()
# self.manual_state = self.manual()nual()
# 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
if debug:
# self.rc_in=[65535, 65535, 65535, 65535, 65535, 1700, 65535, 65535]
Thread(target=self.plot,args=()).start()
# pass
# 启动获取距离
# 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):
while True:
#
if len(self.cloud_points) > 5 and self.arm_state and self.manual_state and (rospy.is_shutdown() is False) :
print ("yunxing")
# if len(self.rc_in)>2 and self.rc_in[5] > 1500 :
# print("yaw",self.yaw)
# self.set_yaw_pid_control(168,rcmsg)
# # print("rcmsg",rcmsg)
# self.rc_override_pub.publish(rcmsg)
self.get_dist_parallel_yaw()
if len(self.pointsClassification[3]) >= 0 and len(self.rc_in)>2 and self.rc_in[5] > 1500 and self.line !=None:
print("延边模式")
first_cortron = True
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.rc_override_pub.publish(rcmsg)
# time.sleep(0.1)
if debug:
# self.print_log(self.line.err) # 打印日志
pass
else:
print("遥控模式")
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)
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()
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 lw =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)
#-----------------------------------------------------------------------#
#-----------------------------------------------------------------------#
# 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)
rospy.logwarn(rc_cmd.channels[2])
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()
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()
# test = []
# # 删除凹陷进去的点
# test.append(paths[0])
# for i in range(2, len(paths)):
# satrt_dist = self.points_dist(paths[i-2], center_point)
# mid_dist = self.points_dist(paths[i-1], center_point)
# end_dist = self.points_dist(paths[i], center_point)
# start_vec = (paths[i-1][0]-paths[i-2][0],
# paths[i-1][1]-paths[i-2][1])
# end_vec = (paths[i][0]-paths[i-1][0],
# paths[i][1]-paths[i-1][1])
# dot = start_vec[0]*end_vec[0]+start_vec[1]*end_vec[1]
# print("satrt_dist", satrt_dist)
# print("mid_dist ", mid_dist)
# print("end_dist ", end_dist)
# print("start_vec ", start_vec)
# print("end_vec", end_vec)
# print("dot ", dot)
# if not((mid_dist > satrt_dist or mid_dist > end_dist) and dot > 0.4):
# test.append(paths[i])
# test.append(paths[-1])
# # 计算K 值 , 在扇形区域
# k = math.tan((90-arefa)/180.0*math.pi)
# # 设置边界
# line = []
# for point in paths:
# # 除去最大k 值
# k1 = None
# if point[0]-center_point[0] != 0:
# k1 = (point[1]-center_point[1]) / \
# (point[0]-center_point[0])
# else:
# k1 = float("inf")
# if k1 < -k or k1 > k:
# line.append(point)
# lines = LineSegment.segment(
# test, center_point=center_point, debug=True, debug_data=paths)
# x = np.array(paths)
paths = paths[::-1]
# print (paths)
slope, intercept, r_value, p_value, std_err = st.linregress(
paths)
# 线性拟合，可以返回斜率，截距，r 值，p 值，标准误差
# slope*x - y + intercept = 0
self.plot(paths, slope, intercept)
deflection = math.atan(slope) * 180.0 / math.pi
print("r_value", r_value**2)
# 船到岸边的距离
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)
# 岸线的平行的角度
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)
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)
def new_rc_msg(yaw, throttle):
message = OverrideRCIn()
message.channels = [yaw, 0, throttle, 0, 0, 0, 0, 0]
return message
def start(self,debug=False):
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)
# 计算单折线斜率
# dist/math.sin(theta)-x/math.tan(theta)
theta,dist,err=LineSegment.segment_Trig(paths,debug=debug)
slope = -1/math.tan(theta)
intercept = dist/math.sin(theta)
# slope, intercept, r_value, p_value, std_err = st.linregress(paths)
# 当线段拟合程度不够时使用双折线拟合
if err >0.5:
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
if debug:
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)
steering = steering_limits[0]
if(throttle > throttle_limits[2]):
throttle = throttle_limits[2]
if(throttle < throttle_limits[0]):
throttle = throttle_limits[0]
rc = OverrideRCIn()
rc.channels = [int(steering), 0, int(throttle), 0, 0, 0, 0, 0]
pub.publish(rc)
if __name__=="__main__":
parser = argparse.ArgumentParser(description='Drive to cone found')
parser.add_argument('--throttle_factor', '-t', default=1.0, type=float,
help='Throttle step size factor')
parser.add_argument('--steering_factor', '-s', default=1.0, type=float,
help='Steering step size factor')
parser.parse_args(rospy.myargv(sys.argv[1:]), args)
try:
pub = rospy.Publisher('/mavros/rc/override', OverrideRCIn, queue_size=10)
rospy.init_node('cone_seeker', anonymous=True)
rospy.Subscriber('/cone_finder/locations', Locations, seek_cone)
rospy.loginfo('cone_seeker init.')
rc = OverrideRCIn()
rc.channels = [1435,0,1500,0,0,0,0,0]
pub.publish(rc)
rospy.spin()
except rospy.ROSInterruptException:
pass
