def thread_offboard(self):
rospy.init_node('offboard', anonymous=True)
self.uav_id = rospy.get_param("~id", "")
rospy.loginfo(self.uav_id + ":start offboard control..")
rate = rospy.Rate(20)
rate.sleep()
rospy.Subscriber(self.uav_id + "/cmd/thrust", Float32,
self.thrustcallback)
rospy.Subscriber(self.uav_id + "/cmd/orientation", Quaternion,
self.quaternioncallback)
rospy.Subscriber(self.uav_id + "/mavros/state", State,
self.statecallback)
rospy.Subscriber(self.uav_id + "/mavros/local_position/pose",
PoseStamped, self.poscallback)
rospy.Subscriber(self.uav_id + "/mavros/local_position/velocity",
TwistStamped, self.velcallback)
arming_client = rospy.ServiceProxy(self.uav_id + "/mavros/cmd/arming",
CommandBool)
pub = rospy.Publisher(self.uav_id + "/mavros/setpoint_raw/attitude",
AttitudeTarget,
queue_size=20)
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
set_mode_client = rospy.ServiceProxy(self.uav_id + "/mavros/set_mode",
SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
if (self.targetattitude.thrust < -1):
rospy.loginfo(" error 00")
break
pub.publish(self.targetattitude)
if ((self.current_state.mode != "OFFBOARD")
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(self.current_state.mode)
if ((set_mode_client.call(offb_set_mode) == True)
and (offb_set_mode.response.success == True)):
rospy.loginfo(self.uav_id + " offboard enabled")
last_request = rospy.get_rostime().secs
else:
if ((self.current_state.armed == False)
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(self.uav_id + " arming...")
if (arming_client.call(arm_cmd).success):
rospy.loginfo(self.uav_id + " armed")
last_request = rospy.get_rostime().secs
rate.sleep()
def _handle_control_mode(self, mode):
"""
Handles the "set_contol_mode" service.
When called, places the FCU into manual flight mode and clears the last accel, vel, and pos setpoints.
"""
res = SetControlModeResponse()
res.success = False
if not self._ready:
return res
self._mode = mode.mode
req = SetModeRequest()
req.base_mode = 0
#req.custom_mode = "19" # MANUAL FLIGHT MODE
req.custom_mode = "0" # STABILIZE FLIGHT MODE
# req.custom_mode = "2" # ALT_HOLD FLIGHT MODE
self._configure_mavros.wait_for_service()
res.success = self._configure_mavros(req).mode_sent
self._control_depth = True
self._control_sway = True
self._control_yaw = True
self._latest_accel_sp = None
self._latest_vel_sp = None
self._latest_pos_sp = None
self._latest_ap_sp = None
self._latest_wp = None
return res
def uav_takeoff(self):
rospy.init_node('takeoff', anonymous=True)
rate = rospy.Rate(20.0)
rate.sleep()
rospy.Subscriber('mavros/state', State, self.state_cb)
local_pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=150)
rospy.Subscriber('mavros/odometry/in',Odometry,self.odometry_cb)
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
setmode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
# rospy.spin()
# print 1
# wait for FCU connected
while (not rospy.is_shutdown()) and (not self.current_state.connected):
rate.sleep()
# print 2
# for i in range(100):
# if not rospy.is_shutdown():
# local_pose_pub.publish(self.pose)
# rate.sleep()
# print i
local_pose_pub.publish(self.pose)
takeoff_set_mode = SetModeRequest()
takeoff_set_mode.base_mode = 0
takeoff_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.get_rostime()
rospy.wait_for_service('mavros/set_mode')
# print 3
# print self.current_state.connected
# print self.current_state.mode
while not rospy.is_shutdown():
if self.current_state.mode != "OFFBOARD" and (rospy.get_rostime() - last_request) > rospy.Duration(3):
if setmode_client.call(takeoff_set_mode).mode_sent:
rospy.loginfo("offboard enabled")
last_request = rospy.get_rostime()
print self.current_state.mode
else:
if (not self.current_state.armed) and (rospy.get_rostime()-last_request) > rospy.Duration(3):
if arming_client.call(arm_cmd).success:
rospy.loginfo("Vehicle armd")
last_request = rospy.get_rostime()
# tf axis has problem ,odo_x for setpoint_y and odo_y for setpoint_x
error_x = abs(self.current_odometry.pose.pose.position.y - self.pose.pose.position.x)
error_y = abs(self.current_odometry.pose.pose.position.x - self.pose.pose.position.y)
error_z = abs(-self.current_odometry.pose.pose.position.z - self.pose.pose.position.z)
error_sum = abs(error_x) + abs(error_y) + abs(error_z)
if error_sum < 0.5:
print "get point"+" "+str([self.pose.pose.position.x, self.pose.pose.position.y, self.pose.pose.position.z])
self.sequence += 1
self.get_waypoint(self.sequence, 5)
self.set_newpoint(self.waypoint_x, self.waypoint_y, self.waypoint_z)
for n in range(60): # delay 3.0s
local_pose_pub.publish(self.pose)
rate.sleep()
elif error_sum >= 0.5:
local_pose_pub.publish(self.pose)
rate.sleep()
local_pub_pos.pose.position.y = 0
local_pub_pos.pose.position.z = 2
count = 0
while not (rospy.is_shutdown() or count > 50):
pub_local_position.publish(local_pub_pos)
print("pub local position 100 ")
rate.sleep()
count += 1
"""change Mode"""
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
set_mode_client = rospy.ServiceProxy("/mavros/set_mode", SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
# pub_gps_geo.publish(gps_pub_geo_msg)
# pub_gps_global.publish(gps_pub_msg)
pub_local_position.publish(local_pub_pos)
if((uav_state.mode != "OFFBOARD") and (rospy.get_rostime().secs-last_request > 2)):
rospy.loginfo(uav_state.mode)
if((set_mode_client.call(offb_set_mode) == True) and (offb_set_mode.response.mode_sent == True)):
rospy.loginfo(" offboard enabled")
last_request = rospy.get_rostime().secs
def uav_takeoff(self):
rospy.init_node('takeoff', anonymous=True)
rate = rospy.Rate(20.0)
rate.sleep()
rospy.Subscriber('mavros/state', State, self.state_cb)
local_pose_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=150)
velocity_pub = rospy.Publisher(
'mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10)
rospy.Subscriber('mavros/odometry/in', Odometry, self.odometry_cb)
home_position_pub = rospy.Publisher('mavros/home_position/home',
HomePosition,
queue_size=10)
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
setmode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
# rospy.spin()
# print 1
# wait for FCU connected
while (not rospy.is_shutdown()) and (not self.current_state.connected):
rate.sleep()
print("state connected")
# for i in range(100):
# if not rospy.is_shutdown():
# local_pose_pub.publish(self.pose)
# rate.sleep()
# print i
local_pose_pub.publish(self.pose)
takeoff_set_mode = SetModeRequest()
takeoff_set_mode.base_mode = 0
takeoff_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.get_rostime()
rospy.wait_for_service('mavros/set_mode')
print("initialized")
# print self.current_state.connected
# print self.current_state.mode
vel_mode = 0
self.vel = self.vel_change(vel_mode)
flag = 0
while not rospy.is_shutdown():
print("in loop")
if self.current_state.mode != "OFFBOARD" and (
rospy.get_rostime() - last_request) > rospy.Duration(3):
if setmode_client.call(takeoff_set_mode).mode_sent:
rospy.loginfo("offboard enabled")
last_request = rospy.get_rostime()
print self.current_state.mode
else:
if (
not self.current_state.armed
) and (rospy.get_rostime() - last_request) > rospy.Duration(3):
if arming_client.call(arm_cmd).success:
rospy.loginfo("Vehicle armd")
last_request = rospy.get_rostime()
if flag == 0:
if abs(self.current_odometry.pose.pose.position.z + 1) > 0.1:
local_pose_pub.publish(self.pose)
rate.sleep()
print(self.current_odometry.pose.pose.position.z)
if abs(self.current_odometry.pose.pose.position.z + 1) < 0.2:
flag = 1
if flag == 1:
for n in range(60): # delay 3.0s
velocity_pub.publish(self.vel)
print(self.vel.linear.x)
rate.sleep()
vel_mode = vel_mode + 1
if vel_mode == 5:
vel_mode = 1
self.vel = self.vel_change(vel_mode)
# tf axis has problem ,odo_x for setpoint_y and odo_y for setpoint_x
# error_x = abs(self.current_odometry.pose.pose.position.y - self.pose.pose.position.x)
# error_y = abs(self.current_odometry.pose.pose.position.x - self.pose.pose.position.y)
# error_z = abs(-self.current_odometry.pose.pose.position.z - self.pose.pose.position.z)
# error_sum = abs(error_x) + abs(error_y) + abs(error_z)
#
# home_position_pub.publish(self.home_position)
#
# if error_sum < 0.5:
# print "get point"+" "+str([self.pose.pose.position.x, self.pose.pose.position.y, self.pose.pose.position.z])
#
# self.sequence += 1
# self.get_waypoint(self.sequence, 5)
# self.set_newpoint(self.waypoint_x, self.waypoint_y, self.waypoint_z)
# for n in range(60): # delay 3.0s
# local_pose_pub.publish(self.pose)
# rate.sleep()
# elif error_sum >= 0.5:
# local_pose_pub.publish(self.pose)
rate.sleep()
def listener():
global current_state
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
rospy.loginfo("start listenning")
rate = rospy.Rate(100)
rate.sleep()
#rospy.Subscriber("/mavros/imu/data", Imu, callback)
#rospy.Subscriber("/uav2/mavros/local_position/pose", PoseStamped, callback)
rospy.Subscriber("/uav2/mavros/state", State, callback)
arming_client = rospy.ServiceProxy("/uav2/mavros/cmd/arming", CommandBool)
pub = rospy.Publisher("/uav2/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
pub1 = rospy.Publisher("/uav2/mavros/setpoint_attitude/attitude",
PoseStamped,
queue_size=10)
#pub=rospy.Publisher("/uav2/mavros/setpoint_position/local",PoseStamped,queue_size=10)
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
pose = Float64()
pose.data = 0.6
att = PoseStamped()
att.pose.orientation.x = 0.5
att.pose.orientation.y = 0.0
att.pose.orientation.z = 0.0
att.pose.orientation.w = 0.866
'''
pose=PoseStamped()
pose.pose.position.x=0
pose.pose.position.y=0
pose.pose.position.z=1
'''
rospy.loginfo("running")
set_mode_client = rospy.ServiceProxy("/uav2/mavros/set_mode", SetMode)
offb_set_mode = SetModeRequest()
offb_set_mode.base_mode = 0
offb_set_mode.custom_mode = "OFFBOARD"
now = rospy.get_rostime()
last_request = now.secs
while not (rospy.is_shutdown()):
pub.publish(pose)
pub1.publish(att)
#rospy.loginfo(current_state.mode)
if ((current_state.mode != "OFFBOARD")
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo(current_state.mode)
rospy.loginfo("1")
if (set_mode_client.call(offb_set_mode).success):
#if(set_mode_client.call(offb_set_mode)==True and offb_set_mode.response.success==True):
rospy.loginfo("offbrd enabled")
last_request = rospy.get_rostime().secs
else:
if ((current_state.armed == False)
and (rospy.get_rostime().secs - last_request > 2)):
rospy.loginfo("2")
if (arming_client.call(arm_cmd).success):
rospy.loginfo("armed")
last_request = rospy.get_rostime().secs
rate.sleep()