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)
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()
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)
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)
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 __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)
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()
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()
# 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
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)
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)
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)
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
%%%%%%%%%%%%%%%%%%%%%%%
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):
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()
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, {}