def set_vel_body_yaw_rate(self, vel_body, yaw_rate, freq):
"""
Offboard method that sends velocity_body and yaw_rate references to the PX4 autopilot of the the drone.
Makes convertions from the body NED frame adopted for the ISR Flying Arena to the body ENU frame used by ROS.
Parameters
----------
vel_body : np.array of floats with shape (3,1)
Desired linear velocity for the drone, in body NED coordinates.
yaw_rate : float
Desired yaw rate for the vehicle, in radians per second.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns +
'/mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
msg = PositionTarget()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 8
msg.type_mask = 1991
msg.velocity.x, msg.velocity.y, msg.velocity.z = ned_to_enu(vel_body)
msg.yaw_rate = -yaw_rate
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def main():
rospy.init_node('offboardX')
rate = rospy.Rate(20)
mavros.set_namespace('/mavros')
state_sub = rospy.Subscriber(mavros.get_topic('state'),
mavros_msgs.msg.State, state_callback)
arming = rospy.ServiceProxy('/mavros/cmd/arming',
mavros_msgs.srv.CommandBool)
#set_mode=rospy.ServiceProxy('/mavros/set_mode',SetMode)
set_mode = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
#local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'),mavros_msgs.msg.PositionTarget,queue_size=10)
local_pub = rospy.Publisher(mavros.get_topic('PositionTarget'),
mavros_msgs.msg.PositionTarget,
queue_size=10)
pose = PositionTarget()
pose.header = Header()
pose.header.frame_id = "att_pose"
pose.header.stamp = rospy.Time.now()
pose.position.x = 0
pose.position.y = 0
pose.position.z = 15
while (not state.connected):
rate.sleep()
for i in range(0, 50):
local_pub.publish(pose)
#mavros.command.arming(True)
#set_mode(0,'OFFBOARD')
last_request = rospy.Time.now()
while 1:
if (state.mode != "OFFBOARD"
and (rospy.Time.now() - last_request > rospy.Duration(5.0))):
print("inside22")
if (set_mode(0, 'OFFBOARD').success):
print("Offboard enabled")
last_request = rospy.Time.now()
else:
if (not state.armed and
(rospy.Time.now() - last_request > rospy.Duration(5.0))):
if (mavros.command.arming(True)):
print("vehicle armed")
last_request = rospy.Time.now()
print("entered")
local_pub.publish(pose)
rospy.spin()
rate.sleep()
return 0
def construct_target(self, x, y, z, yaw, yaw_rate=1):
target_raw_pose = PositionTarget()
target_raw_pose.header = Header()
target_raw_pose.header.stamp = rospy.Time.now()
target_raw_pose.coordinate_frame = 9
target_raw_pose.position.x = x
target_raw_pose.position.y = y
target_raw_pose.position.z = z
target_raw_pose.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ \
+ PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ \
+ PositionTarget.FORCE
target_raw_pose.yaw = yaw
target_raw_pose.yaw_rate = yaw_rate
return target_raw_pose
def main(args):
global list_current_position, list_current_velocity, current_position, usingvelocitycontrol, usingpositioncontrol, xvel, yvel, state, cur_pose, list_error, target
global global_obs_detected, list_velocity_angle, vController
par.CurrentIteration = 1
if (len(args) > 1):
uav_ID = str(args[1])
else:
uav_ID = "1"
idx = int(uav_ID) - 1
rospy.init_node("control_uav_" + uav_ID)
print "UAV" + uav_ID
vController = VelocityController()
try:
rospy.wait_for_service("/uav" + uav_ID + "/mavros/cmd/arming")
rospy.wait_for_service("/uav" + uav_ID + "/mavros/set_mode")
except rospy.ROSException:
fail("failed to connect to service")
state_sub = rospy.Subscriber("/uav" + uav_ID + "/mavros/state",
State,
queue_size=10,
callback=state_cb)
local_vel_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=10)
local_pos_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_position/local",
PoseStamped,
queue_size=10)
local_pos_target = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/local",
PositionTarget,
queue_size=10)
atittude_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_raw/attitude",
AttitudeTarget,
queue_size=10)
thr_pub = rospy.Publisher("/uav" + uav_ID +
"/mavros/setpoint_attitude/att_throttle",
Float64,
queue_size=10)
Astar_grid_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_grid",
GridCells,
queue_size=10)
Astar_path_pub = rospy.Publisher("/uav" + uav_ID + "/Astar_path",
GridCells,
queue_size=10)
arming_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/cmd/arming",
CommandBool)
set_mode_client = rospy.ServiceProxy("/uav" + uav_ID + "/mavros/set_mode",
SetMode)
for i in range(4):
#velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i)
position_sub = rospy.Subscriber("/uav" + repr(i + 1) +
"/mavros/local_position/pose",
PoseStamped,
queue_size=10,
callback=position_cb,
callback_args=(i, int(uav_ID) - 1))
scan_lidar_sub = rospy.Subscriber("/scan",
LaserScan,
queue_size=10,
callback=scan_lidar_cb,
callback_args=uav_ID)
br = tf.TransformBroadcaster()
r = rospy.Rate(10)
print "TRY TO CONNECT"
while ((not rospy.is_shutdown()) and (not current_state.connected)):
#rospy.spinOnce()
r.sleep()
#print(current_state.connected.__class__)
rospy.loginfo("CURRENT STATE CONNECTED")
poses = PoseStamped()
#poses.pose = Pose()
#poses.pose.position = Point()
target = Pose()
if (idx == 0):
target.position.x = 0
target.position.y = 0
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 0
target.position.y = 0
if (idx == 3):
target.position.x = 4
target.position.y = 4
target.position.z = 10
poses.pose.position.x = target.position.x
poses.pose.position.y = target.position.y
poses.pose.position.z = target.position.z
q = quaternion_from_euler(0, 0, 45 * np.pi / 180.0)
poses.pose.orientation.x = q[0]
poses.pose.orientation.y = q[1]
poses.pose.orientation.z = q[2]
poses.pose.orientation.w = q[3]
i = 100
#while((not rospy.is_shutdown()) and (i>0)):
# local_pos_pub.publish(poses)
# i = i-1
rviz_visualization_start = False
last_request = rospy.Time.now()
count = 0
if (idx == 1):
target.position.x = 28
target.position.y = 28
if (idx == 2):
target.position.x = 0
target.position.y = 17
#thread1 = Thread(target = key_function)
#thread1.start()
style.use('fivethirtyeight')
thread2 = Thread(target=plot_error_live)
thread2.start()
thread3 = Thread(target=calculate_and_publish_wall,
args=(Astar_grid_pub, Astar_path_pub, idx))
thread3.start()
#file_error_pos_x = open(dir_path+'/txt/error_pos_x.txt','w')
#file_error_pos_y = open(dir_path+'/txt/error_pos_y.txt','w')
#file_error_pos_z = open(dir_path+'/txt/error_pos_z.txt','w')
error_time = 0
print poses
uav_color = [255, 0, 0, 255]
topic = 'uav_marker_' + uav_ID
uav_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_marker = Marker()
uav_marker.header.frame_id = "world"
uav_marker.type = uav_marker.SPHERE
uav_marker.action = uav_marker.ADD
uav_marker.scale.x = par.ws_model['robot_radius'] * 3
uav_marker.scale.y = par.ws_model['robot_radius'] * 3
uav_marker.scale.z = 0.005 * par.RealScale
uav_marker.color.r = float(uav_color[0]) / 255
uav_marker.color.g = float(uav_color[1]) / 255
uav_marker.color.b = float(uav_color[2]) / 255
uav_marker.color.a = float(uav_color[3]) / 255
uav_marker.pose.orientation.w = 1.0
uav_marker.pose.position.x = 0 #init_pos[0]
uav_marker.pose.position.y = 0 #init_pos[1]
uav_marker.pose.position.z = 0 #init_pos[2]
uav_marker_publisher.publish(uav_marker)
uav_color = [255, 255, 255, 255]
topic = 'uav_goal_marker_' + uav_ID
uav_goal_marker_publisher = rospy.Publisher(topic, Marker, queue_size=10)
uav_goal_marker = Marker()
uav_goal_marker.header.frame_id = "world"
uav_goal_marker.type = uav_goal_marker.SPHERE
uav_goal_marker.action = uav_goal_marker.ADD
uav_goal_marker.scale.x = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.y = par.ws_model['robot_radius'] * 2
uav_goal_marker.scale.z = 0.005 * par.RealScale
uav_goal_marker.color.r = float(uav_color[0]) / 255
uav_goal_marker.color.g = float(uav_color[1]) / 255
uav_goal_marker.color.b = float(uav_color[2]) / 255
uav_goal_marker.color.a = float(uav_color[3]) / 255
uav_goal_marker.pose.orientation.w = 1.0
uav_goal_marker.pose.position.x = 0 #init_pos[0]
uav_goal_marker.pose.position.y = 0 #init_pos[1]
uav_goal_marker.pose.position.z = 0 #init_pos[2]
uav_goal_marker_publisher.publish(uav_goal_marker)
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
last_request_rviz = rospy.Time.now()
last_HRVO_request = rospy.Time.now()
while (not rospy.is_shutdown()):
if (rviz_visualization_start and
(rospy.Time.now() - last_request_rviz > rospy.Duration(0.2))):
publish_uav_position_rviz(
br, list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, uav_ID)
uav_marker = update_uav_marker(
uav_marker, (list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z, 1.0))
uav_marker_publisher.publish(uav_marker)
last_request_rviz = rospy.Time.now()
if ((not rviz_visualization_start)
and (current_state.mode != 'OFFBOARD')
and (rospy.Time.now() - last_request > rospy.Duration(1.0))):
offb_set_mode = set_mode_client(0, 'OFFBOARD')
if (offb_set_mode.mode_sent):
rospy.loginfo("OFFBOARD ENABLED")
last_request = rospy.Time.now()
else:
if ((not current_state.armed) and
(rospy.Time.now() - last_request > rospy.Duration(1.0))):
arm_cmd = arming_client(True)
if (arm_cmd.success):
rospy.loginfo("VEHICLE ARMED")
rviz_visualization_start = True
last_request = rospy.Time.now()
#print distance3D(cur_pose.pose,target)
if (distance3D(cur_pose.pose, target) <
0.5) and (not usingvelocitycontrol):
print "sampai"
usingvelocitycontrol = True
usingpositioncontrol = False
target = Pose()
if (idx == 0):
target.position.x = 28
target.position.y = 28
if (idx == 1):
target.position.x = 0
target.position.y = 0
if (idx == 2):
target.position.x = 21
target.position.y = 14
if (idx == 3):
target.position.x = 0
target.position.y = 0
target.position.z = 10
print target
psi_target = 45 * np.pi / 180.0 #np.pi/180.0 #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
diagram4 = GridWithWeights(par.NumberOfPoints, par.NumberOfPoints)
diagram4.walls = []
for i in range(par.NumberOfPoints
): # berikan wall pada algoritma pathfinding A*
for j in range(par.NumberOfPoints):
if (par.ObstacleRegion[j, i] == 0):
diagram4.walls.append((i, j))
for i in range(par.NumberOfPoints):
for j in range(par.NumberOfPoints):
if (par.ObstacleRegionWithClearence[j][i] == 0):
diagram4.weights.update({(i, j): par.UniversalCost})
goal_pos = (target.position.x / par.RealScale,
target.position.y / par.RealScale)
start_pos = (cur_pose.pose.position.x / par.RealScale,
cur_pose.pose.position.y / par.RealScale)
pathx, pathy = Astar_version1(par, start_pos, goal_pos, diagram4)
print pathx
print pathy
vController.setHeadingTarget(deg2rad(90.0)) #45.0))
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker,
(target.position.x, target.position.y, target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
vController.setTarget(target)
vController.setPIDGainX(1, 0.0, 0.0)
vController.setPIDGainY(1, 0.0, 0.0)
vController.setPIDGainZ(1, 0, 0)
vController.setPIDGainPHI(1, 0.0, 0.0)
vController.setPIDGainTHETA(1, 0.0, 0.0)
vController.setPIDGainPSI(1, 0.0, 0.0)
if (usingpositioncontrol):
print "kontrol posisi"
poses.pose.position.x = 29
poses.pose.position.y = 29
poses.pose.position.z = 10
local_pos_pub.publish(poses)
elif (usingvelocitycontrol):
if (distance2D(cur_pose.pose, target) < 1.5):
if (len(pathx) > 1):
del pathx[0]
del pathy[0]
target.position.x = pathx[0] * par.RealScale
target.position.y = pathy[0] * par.RealScale
uav_goal_marker = update_uav_marker(
uav_goal_marker, (target.position.x, target.position.y,
target.position.z, 1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
print target
vController.setTarget(target)
if (rospy.Time.now() - last_HRVO_request > rospy.Duration(0.05)):
last_HRVO_request = rospy.Time.now()
header.stamp = rospy.Time.now()
des_vel = vController.update(cur_pose)
current_agent_pose = (
list_current_position[idx].pose.position.x,
list_current_position[idx].pose.position.y,
list_current_position[idx].pose.position.z)
current_agent_vel = (list_current_velocity[idx].twist.linear.x,
list_current_velocity[idx].twist.linear.y)
current_neighbor_pose = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_pose.append(
(list_current_position[i].pose.position.x,
list_current_position[i].pose.position.y))
current_neighbor_vel = []
for i in range(par.NumberOfAgents):
if (i != idx):
current_neighbor_vel.append(
(list_current_velocity[i].twist.linear.x,
list_current_velocity[i].twist.linear.y))
V_des = (des_vel.twist.linear.x, des_vel.twist.linear.y)
quat_arr = np.array([
list_current_position[idx].pose.orientation.x,
list_current_position[idx].pose.orientation.y,
list_current_position[idx].pose.orientation.z,
list_current_position[idx].pose.orientation.w
])
att = euler_from_quaternion(quat_arr, 'sxyz')
#V = locplan.RVO_update_single(current_agent_pose, current_neighbor_pose, current_agent_vel, current_neighbor_vel, V_des, par,list_velocity_angle,list_distance_obs,att[2])
V, RVO_BA_all = locplan.RVO_update_single_static(
current_agent_pose, current_neighbor_pose,
current_agent_vel, current_neighbor_vel, V_des, par)
V_msg = des_vel
V_msg.twist.linear.x = V[0]
V_msg.twist.linear.y = V[1]
local_vel_pub.publish(V_msg)
goal = (target.position.x, target.position.y,
target.position.z)
x = current_position
list_error[0].append(error_time)
list_error[1].append(goal[0] - x[0])
list_error[2].append(goal[1] - x[1])
list_error[3].append(goal[2] - x[2])
error_time = error_time + 1
k = 0.1
vx = k * (goal[0] - x[0])
vy = k * (goal[1] - x[1])
vz = k * (goal[2] - x[2])
postar = PositionTarget()
postar.header = header
postar.coordinate_frame = PositionTarget().FRAME_LOCAL_NED
p = PositionTarget().IGNORE_PX | PositionTarget(
).IGNORE_PY | PositionTarget().IGNORE_PZ
a = PositionTarget().IGNORE_AFX | PositionTarget(
).IGNORE_AFY | PositionTarget().IGNORE_AFZ
v = PositionTarget().IGNORE_VX | PositionTarget(
).IGNORE_VY | PositionTarget().IGNORE_VZ
y = PositionTarget().IGNORE_YAW | PositionTarget(
).IGNORE_YAW_RATE
f = PositionTarget().FORCE
postar.type_mask = a | y | p | f #| PositionTarget().IGNORE_YAW | PositionTarget().IGNORE_YAW_RATE | PositionTarget().FORCE
postar.velocity.x = vx
postar.velocity.y = vy
postar.velocity.z = vz
postar.position.x = goal[0]
postar.position.y = goal[1]
postar.position.z = goal[2]
vel_msg = TwistStamped()
vel_msg.header = header
vel_msg.twist.linear.x = xvel
vel_msg.twist.linear.y = yvel
vel_msg.twist.linear.z = 0.0
vel_msg.twist.angular.x = 0.0
vel_msg.twist.angular.y = 0.0
vel_msg.twist.angular.z = 0.0
q = quaternion_from_euler(0, 0, 0.2)
att = PoseStamped()
att.pose.orientation.x = q[0]
att.pose.orientation.y = q[1]
att.pose.orientation.z = q[2]
att.pose.orientation.w = q[3]
att.pose.position.x = 0.0
att.pose.position.y = 0.0
att.pose.position.z = 2.0
cmd_thr = Float64()
cmd_thr.data = 0.3
att_raw = AttitudeTarget()
att_raw.header = Header()
att_raw.body_rate = Vector3()
att_raw.thrust = 0.7 #Float64()
att_raw.header.stamp = rospy.Time.now()
att_raw.header.frame_id = "fcu"
att_raw.type_mask = 71
att_raw.orientation = Quaternion(
*quaternion_from_euler(0, 0, 0.2))
else:
local_pos_pub.publish(poses)
count = count + 1
r.sleep()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
