def inspect_wt(self, duration=rospy.Duration(600, 0)):
"""
ASV inspection for a WT that involves pointing a camera
to the WT while ASV is moving back and forth
"""
fuel = self.fuel
start = rospy.Time.now()
# position where drone is originated in one of the wps
wp = self.waypoints[self._current_wp - 1]
original = GlobalPositionTarget()
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
original.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT
original.type_mask = 0b001111111000
original.latitude = wp['lat']
original.longitude = wp['long']
original.altitude = 0.0
original.yaw = yaw_ned_to_enu(wp['yaw'])
original.yaw_rate = 2.0
rospy.loginfo("%s is scanning a wind turbine..." % self.namespace)
first_blade = self.blade_inspect(original, [0.0, 90.0, 0.0], duration)
self.calculate_fuel_rate(fuel, start, self.fuel_rate_std)
rospy.loginfo("%s has done the inspection..." % self.namespace)
return first_blade
def task_allocation():
global wp_publisher, vehicle_pos, rate
task_list = task_manager.get_tasks()
mission_complete = False
task_in_progress = False
print "Node Begins"
while not mission_complete:
if not task_in_progress:
print "Task Allocation Begin\n"
task = task_manager.get_optimal_task(task_list, vehicle_pos)
target = GlobalPositionTarget()
target.coordinate_frame = 6
target.type_mask = 4088
target.latitude = task[0]
target.longitude = task[1]
target.altitude = 5.0
target.header.frame_id = '/base_link'
wp_publisher.publish(target)
task_in_progress = True
print "Task Alloted\n"
if task_in_progress and task_manager.task_achieved(task, vehicle_pos):
print "Removing Task from List"
task_in_progress = False
task_list.remove(task)
if (len(task_list) == 0):
mission_complete = True
rate.sleep()
def goto(self,
waypoint,
duration=rospy.Duration(60, 0),
low_fuel_trip=False):
"""
Go to specific waypoint action
"""
init_fuel = self.fuel
start = rospy.Time.now()
wp = self.waypoints[waypoint - 1]
original = GlobalPositionTarget()
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
original.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT
original.type_mask = 0b001111111000
original.latitude = wp['lat']
original.longitude = wp['long']
original.altitude = 0.0
original.yaw = yaw_ned_to_enu(wp['yaw'])
original.yaw_rate = 2.0
rospy.loginfo("%s is going to waypoint %d, lat: %.5f, long: %.5f" %
(self.namespace, waypoint, wp['lat'], wp['long']))
reached_original = self.goto_coordinate(original,
low_fuel_trip=low_fuel_trip,
radius=self._radius,
duration=duration)
rospy.loginfo("%s is reaching waypoint %d, lat: %.5f, long: %.5f" %
(self.namespace, waypoint, wp['lat'], wp['long']))
self.calculate_fuel_rate(init_fuel, start, self.fuel_rate_std)
return reached_original
def next_waypoint(vehicle_number, latitude, longitude):
publisher = rospy.Publisher('/vehicle' + str(vehicle_number) + '/mavros/setpoint_raw/global', GlobalPositionTarget, queue_size=10)
pub = GlobalPositionTarget()
pub.coordinate_frame = 6
pub.type_mask = 4088
pub.longitude = longitude
pub.latitude = latitude
publisher.publish(pub)
def blade_inspect(self,
original,
target_position,
duration=rospy.Duration(300, 0)):
"""
Inspecting the blade given the [original] pose to return to
and end [target] position to scan
"""
start = rospy.Time.now()
reached_original = self.ACTION_FAIL
# rotate the camera
for _ in range(3):
self._rotate_cam.publish(Int32(1))
rospy.sleep(0.1)
# position where ASV is supposed to be
heading = self.heading
offset_x = (target_position[0] * np.cos(heading) +
target_position[1] * np.sin(heading))
offset_y = (-1 * target_position[0] * np.sin(heading) +
target_position[1] * np.cos(heading))
latitude_offset, longitude_offset = xy_to_longlat(
offset_x, offset_y, self.global_pose.latitude)
target = GlobalPositionTarget()
target.header.seq = 1
target.header.stamp = rospy.Time.now()
target.header.frame_id = 'map'
target.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_REL_ALT
target.type_mask = 0b001111111000
target.latitude = self.global_pose.latitude + latitude_offset
target.longitude = self.global_pose.longitude + longitude_offset
target.altitude = target_position[2]
target.yaw = yaw_ned_to_enu(heading)
target.yaw_rate = 2.0
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
reached_target = self.goto_coordinate(target,
low_fuel_trip=False,
duration=duration)
rospy.loginfo("%s is returning to %3.5f, %3.5f position..." %
(self.namespace, original.latitude, original.longitude))
if reached_target == self.ACTION_SUCCESS:
# rotate the camera
for _ in range(3):
self._rotate_cam.publish(Int32(-1))
rospy.sleep(0.1)
original.header.seq = 1
original.header.stamp = rospy.Time.now()
original.header.frame_id = 'map'
duration = duration - (rospy.Time.now() - start)
start = rospy.Time.now()
reached_original = self.goto_coordinate(original,
low_fuel_trip=False,
duration=duration)
# rotate the camera
for _ in range(3):
self._rotate_cam.publish(Int32(0))
rospy.sleep(0.1)
return np.min([reached_target, reached_original])
def reaching_cb(user_data):
rospy.loginfo('mode GUIDED')
setGuidedMode()
rospy.loginfo('Balloon found')
rospy.loginfo('Mission Reaching')
(locked, err_x_pix, err_y_pix, err_x_m, err_y_m, dist, res_w,
res_h) = balloonchecker()
#position_topic = rospy.Subscriber('/mavros/global_position/global', NavSatFix, home_callback)
#Alt_topic = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_callback)
#compass_topic = rospy.Subscriber('mavros/global_position/compass_hdg', Float64, compass_callback)
#rospy.sleep(0.5)
if locked == 1:
vel_x, vel_y, vel_z, yaw_rate = dir_pid_return()
set_velocity_body(vel_x, vel_y, vel_z, yaw_rate)
while locked == 1 and mode == 'GUIDED':
vel_x, vel_y, vel_z, yaw_rate = dir_pid_return()
set_velocity_body(vel_x, vel_y, vel_z, yaw_rate)
(locked, err_x_pix, err_y_pix, err_x_m, err_y_m, dist, res_w,
res_h) = balloonchecker()
if locked == 1 and abs(err_x_m) <= 0.1 and abs(err_y_m) < 0.1 and abs(
dist) < 1.2:
break
if mode == 'ALT_HOLD':
Clear_Mission()
rospy.signal_shutdown('Quit for Alt_hold')
os.system('rosnode kill ' + mavros_name)
sis.stop()
elif locked == 1 and abs(err_x_m) <= 0.1 and abs(err_y_m) < 0.1 and abs(
dist) < 1.2:
#if i got it in my hands
my_latit = lat
my_longit = lon
#my_new_altit = alt + user_data.delta_alt
my_new_altit = alt
set_position = GlobalPositionTarget()
set_position.coordinate_frame = 6
set_position.latitude = my_latit
set_position.longitude = my_longit
set_position.altitude = my_new_altit
set_position.type_mask = 4088
setpoint_position_pub.publish(set_position)
time.sleep(0.5)
print 'STOP'
print 'Balloon reached!'
return 'searching'
elif locked == 0:
# user_data.target_loc = target_loc
# print target_loc
return 'searching'
else:
return 'failed'
def action_cb(user_data):
rospy.loginfo('Mission Action')
position_topic = rospy.Subscriber('/mavros/global_position/global', NavSatFix, home_callback)
Alt_topic = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_callback)
compass_topic = rospy.Subscriber('mavros/global_position/compass_hdg', Float64, compass_callback)
(locked, err_x_pix, err_y_pix, target_dx, target_dy, target_dist_normal, target_yaw, theta_err) = rec_and_show.ballonchecker()
rospy.sleep(0.5)
time_ini = rospy.Time.now().secs
print 'Action: count'
while (rospy.Time.now().secs-time_ini) < config.getint('GAPL','action_time') and mode == 'GUIDED':
(locked, err_x_pix, err_y_pix, target_dx, target_dy, target_dist_normal, target_yaw, theta_err) = rec_and_show.ballonchecker()
if locked == 1:
vel_x = 1.1*user_data.k_action*(target_dist_normal-user_data.dist_min)
vel_x = saturate(vel_x, user_data.vel_x_min, user_data.vel_x_max, user_data.v_zero)
vel_y = user_data.kerr_vely*target_dx
vel_y = saturate(vel_y, user_data.vel_y_min, user_data.vel_y_max, user_data.v_zero)
vel_z = -user_data.k_z*(target_dy)
vel_z = saturation_velZ(vel_z, alt, user_data.alt_min, user_data.alt_max, target_dy, user_data.Hmax85, user_data.Hmin115)
psi_dot = (((user_data.yaw_rateMax-user_data.yaw_rateMin)/(user_data.errx_max_pix-user_data.errx_min_pix))*(abs(err_x_pix)-user_data.errx_min_pix))+user_data.yaw_rateMin
vel_yaw = yaw_alignment(err_x_pix, user_data.errx_min_pix, psi_dot)
print "Vx=%.2f m/sec" % (vel_x),"; Vy=%.2f m/sec" % (vel_y),"; Vz=%.2f m/sec" % (vel_z),"; yaw_rate=%.2f rad/s\r" % (vel_yaw),
sys.stdout.flush()
set_velocity_body(vel_x, vel_y, vel_z, vel_yaw)
bearing = add_angles(compass, target_yaw)
target_loc = target_location(target_dist_normal, target_dx, target_dy, bearing, lat, lon, alt)
if locked == 1:
balloon_position = target_loc
position_memory.balloon_reached(balloon_position[0], balloon_position[1], balloon_position[2])
print 'Balloon added to the list of reached'
my_latit = lat
my_longit = lon
my_new_altit = alt + user_data.delta_alt
set_position = GlobalPositionTarget()
set_position.coordinate_frame = 6
set_position.latitude = my_latit
set_position.longitude = my_longit
set_position.altitude = my_new_altit
set_position.type_mask = 4088
setpoint_position_pub.publish(set_position)
time.sleep(0.5)
print 'Changing Altitude'
while alt < my_new_altit*0.85 and mode == 'GUIDED':
continue
if mode == 'ALT_HOLD':
Clear_Mission()
print 'Stop VideoGet and VideoShow'
rec_and_show.stop()
rospy.signal_shutdown('Quit for Alt_hold')
os.system('rosnode kill mavros')
sis.stop()
elif mode == 'GUIDED' and alt >= my_new_altit*0.85:
print 'New Altitude Reached'
return 'finished'
else:
return 'failed'
def publish_target_pose(self, pose_s):
coords = self.pose2wgs84(pose_s)
if not coords:
rospy.logerr('Cannot compute pose in UTM frame')
return
lat, lon, heading = coords
msg = GlobalPositionTarget()
msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT
msg.type_mask = (GlobalPositionTarget.IGNORE_VX +
GlobalPositionTarget.IGNORE_VY +
GlobalPositionTarget.IGNORE_VZ +
GlobalPositionTarget.IGNORE_AFX +
GlobalPositionTarget.IGNORE_AFY +
GlobalPositionTarget.IGNORE_AFZ +
GlobalPositionTarget.IGNORE_YAW +
GlobalPositionTarget.IGNORE_YAW_RATE)
msg.latitude = lat
msg.longitude = lon
msg.altitude = 0.0
msg.yaw = heading
self.pub.publish(msg)
self.pub_pose.publish(pose_s)
def publish_target_pose(self, pose_s):
# rospy.loginfo('publish_target_pose %s', pose_s)
utm_pose_s = pose_in_frame(self.tf_buffer, pose_s, 'utm')
if not utm_pose_s:
rospy.logerr('Cannot compute pose in UTM frame')
return
lat, lon, heading = pose2wgs84(utm_pose_s)
msg = GlobalPositionTarget()
msg.coordinate_frame = self.coordinate_frame()
msg.type_mask = (GlobalPositionTarget.IGNORE_VX +
GlobalPositionTarget.IGNORE_VY +
GlobalPositionTarget.IGNORE_VZ +
GlobalPositionTarget.IGNORE_AFX +
GlobalPositionTarget.IGNORE_AFY +
GlobalPositionTarget.IGNORE_AFZ +
GlobalPositionTarget.IGNORE_YAW +
GlobalPositionTarget.IGNORE_YAW_RATE)
msg.latitude = lat
msg.longitude = lon
msg.altitude = self.altitude(utm_pose_s.pose.position.z)
# msg.yaw = heading
self.global_pub.publish(msg)
super(MAVPathFollower, self).publish_target_pose(pose_s)
def publish_target_pose(self, pose):
point = _a(pose.pose.position)
# print 'point' , point
yaw = np.arctan2(pose.pose.orientation.z,
pose.pose.orientation.w) * 2.0
latlon, heading = self.pose2wgs84(point, yaw)
lon, lat = latlon.tolist()[0]
# print lon, lat
msg = GlobalPositionTarget()
msg.coordinate_frame = GlobalPositionTarget.FRAME_GLOBAL_INT
msg.type_mask = (GlobalPositionTarget.IGNORE_VX +
GlobalPositionTarget.IGNORE_VY +
GlobalPositionTarget.IGNORE_VZ +
GlobalPositionTarget.IGNORE_AFX +
GlobalPositionTarget.IGNORE_AFY +
GlobalPositionTarget.IGNORE_AFZ +
GlobalPositionTarget.IGNORE_YAW +
GlobalPositionTarget.IGNORE_YAW_RATE)
msg.latitude = lat
msg.longitude = lon
msg.altitude = 0.0
msg.yaw = heading
self.pub.publish(msg)
self.pub_pose.publish(pose)
wp2 = GlobalPositionTarget() wp3 = GlobalPositionTarget() wp4 = GlobalPositionTarget() wp = GlobalPositionTarget() wp_target = GlobalPositionTarget() wp_avoidance = GlobalPositionTarget() wp_helical = GlobalPositionTarget() wp0.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_INT wp0.type_mask = GlobalPositionTarget().IGNORE_VX + GlobalPositionTarget( ).IGNORE_VY + GlobalPositionTarget().IGNORE_VZ + GlobalPositionTarget( ).IGNORE_AFX + GlobalPositionTarget().IGNORE_AFY + GlobalPositionTarget( ).IGNORE_AFZ + GlobalPositionTarget().FORCE + GlobalPositionTarget( ).IGNORE_YAW_RATE wp0.latitude = 37.565011 wp0.longitude = 126.628919 wp0.altitude = 5.0 wp0.yaw = np.pi / 180.0 * 117.0 wp1.coordinate_frame = wp0.coordinate_frame wp1.type_mask = wp0.type_mask wp1.latitude = 37.566025 wp1.longitude = 126.628206 wp1.altitude = 100.0 wp1.yaw = np.pi / 180.0 * 117.0 wp2.coordinate_frame = wp0.coordinate_frame wp2.type_mask = wp0.type_mask wp2.latitude = 37.565350 wp2.longitude = 126.626778
def task_allocation():
global wp_publisher1, wp_publisher2, vehicle1_pos, vehicle2_pos, rate
task_list = task_manager.get_tasks()
tasks_finished = False
vehicle1_in_progress = False
vehicle2_in_progress = False
vehicle1_task_complete = False
vehicle2_task_complete = False
print "Node Begins"
while (not vehicle1_task_complete) or (not vehicle2_task_complete) or (
not tasks_finished):
if not vehicle1_in_progress and not tasks_finished:
print "Task Allocation Begin\n"
vehicle1_task = task_manager.get_optimal_task(
task_list, vehicle1_pos)
target = GlobalPositionTarget()
target.coordinate_frame = 6
target.type_mask = 4088
target.latitude = vehicle1_task[0]
target.longitude = vehicle1_task[1]
target.altitude = 5.0
target.header.frame_id = ''
wp_publisher1.publish(target)
vehicle1_in_progress = True
vehicle1_task_complete = False
task_list.remove(vehicle1_task)
if (len(task_list) == 0):
tasks_finished = True
print "Task Alloted to Vehicle 1\n"
if not vehicle2_in_progress and not tasks_finished:
print "Task Allocation Begin\n"
vehicle2_task = task_manager.get_optimal_task(
task_list, vehicle2_pos)
target = GlobalPositionTarget()
target.coordinate_frame = 6
target.type_mask = 4088
target.latitude = vehicle2_task[0]
target.longitude = vehicle2_task[1]
target.altitude = 5.0
target.header.frame_id = ''
wp_publisher2.publish(target)
vehicle2_in_progress = True
vehicle2_task_complete = False
task_list.remove(vehicle2_task)
if (len(task_list) == 0):
tasks_finished = True
print "Task Alloted to Vehicle 2\n"
if vehicle1_in_progress and task_manager.task_achieved(
vehicle1_task, vehicle1_pos):
print "Vehicle1 achieved task"
vehicle1_in_progress = False
vehicle1_task_complete = True
if vehicle2_in_progress and task_manager.task_achieved(
vehicle2_task, vehicle2_pos):
print "Vehicle2 achieved task"
vehicle2_in_progress = False
vehicle2_task_complete = True
#print "Vehicle 1 task comlpete", vehicle1_task_complete
#print "Vehicle 2 task comlpete", vehicle2_task_complete
#print "task comlpete", tasks_finished
rate.sleep()
print "All tasks finished"
def cbPubCmd(self, event):
if not self.started: return
if self.home_pos[2] < 1: return # Wait for home pos to be received
# Check if distance to waypoint is small enough to count it as reached TODO may be wrong approximation and TODO change for RTK
if self.getDistanceBetweenGPS(self.current_pos,
self.waypoints[self.currentWPidx]) < 2:
self.currentWPidx += 1
if self.currentWPidx == self.maxWPidx:
rospy.loginfo("MISSION DONE")
self.started = False
return
# Log info about current waypoint
rospy.loginfo("Current waypoint: " + str(self.currentWPidx) + " / " +
str(self.maxWPidx))
# Check if mode needs to be changed for OFFBOARD and ARM vehicle (this is a startup procedure)
if str(self.current_state.mode) != "OFFBOARD" and rospy.Time.now(
) - self.last_request > rospy.Duration(5.0):
resp = self.set_mode_client(0, "OFFBOARD")
if resp.mode_sent:
rospy.loginfo("Offboard enabled")
self.last_request = rospy.Time.now()
else:
if not self.current_state.armed and rospy.Time.now(
) - self.last_request > rospy.Duration(5.0):
resp = self.arming_client(True)
if resp.success:
rospy.loginfo("Vehicle armed")
self.last_request = rospy.Time.now()
# Publish information - where should the drone fly next?
pose = PoseStamped()
latWP = self.waypoints[self.currentWPidx][0]
lonWP = self.waypoints[self.currentWPidx][1]
altWP = self.waypoints[self.currentWPidx][2]
velWP = self.waypoints[self.currentWPidx][3]
# latHome = self.home_pos[0]
# lonHome = self.home_pos[1]
# altHome = self.home_pos[2]
# pose.pose.position.x = 6371000 * radians(lonWP - lonHome) * cos(radians(latHome))
# pose.pose.position.y = 6371000 * radians(latWP - latHome)
# pose.pose.position.z = altWP - altHome
#self.local_pos_pub.publish(pose)
if self.ready:
msg = GlobalPositionTarget()
msg.latitude = latWP
msg.longitude = lonWP
msg.altitude = altWP
msg.header.stamp = rospy.Time.now()
msg.coordinate_frame = 5
msg.type_mask = 0b101111111000
msg.yaw = self.getNextYaw()
self.global_pos_pub.publish(msg)
par = ParamValue()
par.integer = 0
par.real = velWP
try:
self.set_vel("MPC_XY_VEL_MAX", par)
except Exception:
print("e")
else:
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
pose.pose.position.z = 2.0
self.local_pos_pub.publish(pose)
try:
par = ParamValue()
par.integer = 0
par.real = velWP
resp = self.set_vel("MPC_XY_VEL_MAX", par)
if resp.success: self.ready = True
except Exception as e:
print(e)
lat_delta = 0
print("stop")
else:
if (key == '\x03'):
break
"""for the no press key time"""
if status == 0:
gps_target_lat = gps_curr.latitude
gps_target_lon = gps_curr.longitude
""" calculate gps msg """
gps_pub_msg.latitude = gps_curr.latitude
gps_pub_msg.longitude = gps_curr.longitude
gps_pub_msg.altitude = gps_curr.altitude # height is acquired by lidar lite v3
""" if new command or not """
if abs(lat_delta) > lat_base*0.5/lat_denominator or abs(lon_delta) > lon_base*0.5/lon_denominator: # if true, move, otherwise, keep
print("new command")
gps_target_lat = gps_pub_msg.latitude + lat_delta
gps_target_lon = gps_pub_msg.longitude + lon_delta
""" if reach target position or not """
if abs(gps_target_lat - gps_curr.latitude) > lat_base*0.2/lat_denominator or abs(gps_target_lon - gps_curr.longitude) > lon_base*0.2/lon_denominator:
print("reaching target ... ")
gps_pub_msg.latitude = gps_target_lat
gps_pub_msg.longitude = gps_target_lon
rate = rospy.Rate(20)
while (not current_state.connected):
print(current_state.connected)
rate.sleep()
print("Creating global position")
point = GlobalPositionTarget()
point.header.stamp = rospy.Time.now()
point.coordinate_frame = GlobalPositionTarget().FRAME_GLOBAL_TERRAIN_ALT
point.type_mask = GlobalPositionTarget().IGNORE_VX + GlobalPositionTarget(
).IGNORE_VY + GlobalPositionTarget().IGNORE_VZ + GlobalPositionTarget(
).IGNORE_AFX + GlobalPositionTarget().IGNORE_AFY + GlobalPositionTarget(
).IGNORE_AFZ + GlobalPositionTarget().FORCE + GlobalPositionTarget(
).IGNORE_YAW_RATE
point.latitude = 37.5647106
point.longitude = 126.6276294
point.altitude = 25.0657
point.yaw = 0.0
for i in range(100):
#local_pos_pub.publish(pose)
point.header.stamp = rospy.Time.now()
global_pos_pub.publish(point)
rate.sleep()
print("Creating Objects for services")
offb_set_mode = SetMode()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBool()
arm_cmd.value = True
def do_helix_trajectory(self):
while (not self.home_pose_set) or (not self.global_home_pose_set):
# do nothing, wait for node initialization to finish
a = 1
ladder_position = self.get_ladder_location()
init_x = self.home_pose.pose.position.x #self.state.pose.pose.position.x
init_y = self.home_pose.pose.position.y #self.state.pose.pose.position.y
init_z = self.home_pose.pose.position.z #self.state.pose.pose.position.z
print("Initial helix position: \n x: {}, y: {}, z: {}\nTime: {}\n".
format(init_x, init_y, init_z,
rospy.Time.now().to_sec()))
self.helix_controller = htc(vrate=0.1,
radius=2.0,
center=(1, 0, 0),
init=(init_x, init_y, init_z),
t_init=rospy.Time.now().to_sec(),
w=0.2) # init with any parameters
self.helix_controller.set_helix_center(ladder_position)
self.yaw_controller = sc2(Kp=6., Kv=0.0)
q = self.state.pose.pose.orientation
q = [q.x, q.y, q.z, q.w]
yaw, pitch, roll = tt.euler_from_quaternion(q, axes='rzyx')
self.helix_controller.phase = yaw + np.pi # yaw angle + 180 degrees because its center perspective
print("Phase: ", self.helix_controller.phase)
rate = rospy.Rate(20)
while (self.state.pose.pose.position.z <
(init_z + self.ladder_height + self.ladder_safety_margin)):
# state for x and y position
xs = np.array([[self.state.pose.pose.position.x],
[self.state.twist.twist.linear.x], [0.0]])
ys = np.array([[self.state.pose.pose.position.y],
[self.state.twist.twist.linear.y], [0.0]])
states = [xs, ys]
#ladder_position = self.get_ladder_location()
#ladder_position = [-29.5,12.5]
#self.helix_controller.set_helix_center(ladder_position)
ux, uy, uz, ref = self.helix_controller.compute_command(
states,
rospy.Time.now().to_sec())
ux, uy = (ref[0][0][0], ref[1][0][0])
q, cyaw = self.compute_yaw2(ladder_position)
self.command_pose.header.stamp = rospy.Time.now()
self.command_pose.pose.position.x = ux
self.command_pose.pose.position.y = uy
self.command_pose.pose.position.z = uz
self.command_pose.pose.orientation.x = q[0]
self.command_pose.pose.orientation.y = q[1]
self.command_pose.pose.orientation.z = q[2]
self.command_pose.pose.orientation.w = q[3]
#self.local_pos_pub.publish(self.command_pose)
# publish reference trajectory in local frame
refmsg = PoseStamped()
refmsg.header.stamp = rospy.Time.now()
refmsg.pose.position.x = ref[0][0][0]
refmsg.pose.position.y = ref[1][0][0]
refmsg.pose.position.z = uz
self.ref_pub_local.publish(refmsg)
# publish reference trajectory in global frame
lat, lon = self.get_global_coordinates(ux, uy)
global_refmsg = GlobalPositionTarget()
global_refmsg.header.stamp = rospy.Time.now()
global_refmsg.header.frame_id = self.global_state.header.frame_id
global_refmsg.latitude = lat
global_refmsg.longitude = lon
global_refmsg.yaw = cyaw
global_refmsg.altitude = uz #(uz - init_z) + self.global_home.altitude
self.ref_pub_global.publish(global_refmsg)
rate.sleep()
def do_helix_trajectory(self):
init_x = self.state.pose.pose.position.x
init_y = self.state.pose.pose.position.y
init_z = self.state.pose.pose.position.z
self.helix_controller = htc(vrate=0.05,
radius=1.0,
center=(1, 0, 0),
init=(init_x, init_y, init_z),
t_init=rospy.get_time(),
w=0.5)
self.yaw_controller = sc2(Kp=6., Kv=0.0)
q = self.state.pose.pose.orientation
q = [q.x, q.y, q.z, q.w]
yaw, pitch, roll = tt.euler_from_quaternion(q, axes='rzyx')
self.helix_controller.phase = yaw + np.pi # yaw angle + 180 degrees because its center perspective
print("Phase: ", self.helix_controller.phase)
rate = rospy.Rate(20)
# do helix trajectory for 30 seconds
print("Doing helix trajectory")
#while( (rospy.get_time() - self.helix_controller.t_init) < 120.):
while (self.state.pose.pose.position.z < (self.ladder_height + 2)):
# state for x and y position
xs = np.array([[self.state.pose.pose.position.x],
[self.state.twist.twist.linear.x], [0.0]])
ys = np.array([[self.state.pose.pose.position.y],
[self.state.twist.twist.linear.y], [0.0]])
states = [xs, ys]
ladder_position = self.get_ladder_location()
ladder_position = [0.0, 1.0]
self.helix_controller.set_helix_center(ladder_position)
ux, uy, uz, ref = self.helix_controller.compute_command(
states, rospy.get_time())
q = self.compute_yaw()
self.command_pose.header.stamp = rospy.Time.now()
self.command_pose.pose.position.x = ux
self.command_pose.pose.position.y = uy
self.command_pose.pose.position.z = uz
self.command_pose.pose.orientation.x = q[0]
self.command_pose.pose.orientation.y = q[1]
self.command_pose.pose.orientation.z = q[2]
self.command_pose.pose.orientation.w = q[3]
self.local_pos_pub.publish(self.command_pose)
# publish reference trajectory in local frame
refmsg = PoseStamped()
refmsg.header.stamp = rospy.Time.now()
refmsg.pose.position.x = ref[0][0][0]
refmsg.pose.position.y = ref[1][0][0]
refmsg.pose.position.z = uz
self.ref_pub_local.publish(refmsg)
# publish reference trajectory in global frame
lat, lon = self.get_global_coordinates(ux, uy)
global_refmsg = GlobalPositionTarget()
global_refmsg.header.stamp = rospy.Time.now()
global_refmsg.header.frame_id = self.global_state.header.frame_id
global_refmsg.latitude = lat
global_refmsg.longitude = lon
global_refmsg.altitude = uz
self.ref_pub_global.publish(global_refmsg)
rate.sleep()