def control_loop(self, driver, time_out):
manager = yield From(pygazebo.connect())
yield From(trollius.sleep(0.5))
world_subscriber = manager.subscribe('/gazebo/default/world_stats', 'gazebo.msgs.WorldStatistics', self.world_callback)
location_subscriber = manager.subscribe('/gazebo/default/pose/info', 'gazebo.msgs.PosesStamped', self.location_callback)
light_sensor1_subscriber = manager.subscribe('/gazebo/default/husky/camera1/link/camera/image', 'gazebo.msgs.ImageStamped', self.light_1_callback)
light_sensor2_subscriber = manager.subscribe('/gazebo/default/husky/camera2/link/camera/image', 'gazebo.msgs.ImageStamped', self.light_2_callback)
laser_subscriber = manager.subscribe('/gazebo/default/husky/hokuyo/link/laser/scan', 'gazebo.msgs.LaserScanStamped', self.laser_callback)
yield From(trollius.sleep(1))
world_publisher = yield From(manager.advertise('/gazebo/default/world_control','gazebo.msgs.WorldControl'))
world_publisher.wait_for_listener()
wheel_publisher = yield From(manager.advertise('/gazebo/default/husky/joint_cmd', 'gazebo.msgs.JointCmd'))
wheel_publisher.wait_for_listener()
world_control = WorldControl()
world_control.pause = True
world_control.reset.all = True
yield From(trollius.sleep(0.01))
yield From(world_publisher.publish(world_control))
left_wheel = JointCmd()
left_wheel.name = 'husky::front_left_joint'
right_wheel = JointCmd()
right_wheel.name = 'husky::front_right_joint'
left_wheel.velocity.target = 0
right_wheel.velocity.target = 0
yield From(trollius.sleep(0.01))
yield From(wheel_publisher.publish(left_wheel))
yield From(wheel_publisher.publish(right_wheel))
world_control.pause = False
yield From(trollius.sleep(0.01))
yield From(world_publisher.publish(world_control))
yield From(trollius.sleep(0.5))
start_time = self.sim_time
end_time = start_time + time_out
print "Starting control loop"
while (self.sim_time < end_time) and (self.distance_to_goal > 0.5):
sensor_input = [self.light_sensor1, self.light_sensor2]
sensor_input += self.collide
(left,right) = driver(sensor_input)
left_wheel.velocity.target = left
right_wheel.velocity.target = right
yield From(wheel_publisher.publish(left_wheel))
yield From(wheel_publisher.publish(right_wheel))
self.update_distance()
if self.distance_to_goal < 0.5:
break
yield From(trollius.sleep(.01))
yield From(trollius.sleep(0.01))
world_control.pause = True
yield From(world_publisher.publish(world_control))
self.left_velocity = left_wheel.velocity.target
self.right_velocity = right_wheel.velocity.target
self.loop.stop()
async def asyland():
print('-- Landing')
unpause_msg = WorldControl()
unpause_msg.pause = False
await asyncio.sleep(0.5)
await pub_world_control.publish(unpause_msg)
await asyncio.sleep(0.5)
await drone.action.land()
async def step_async(action):
step_msg = WorldControl()
step_msg.step = True
# await asyncio.sleep(0.1)
await pub_world_control.publish(step_msg) ## perform pone step in gazebo
### for attitude contorl
action = [0, 0, 0, action]
### for attitude contorl
await drone.offboard.set_attitude_rate(
AttitudeRate(action[0], action[1], action[2],
action[3])) ## publish action in deg/s, thrust [0:1]
async def reset_async(reset_pos):
unpause_msg = WorldControl()
unpause_msg.pause = False
await asyncio.sleep(0.001)
await pub_world_control.publish(unpause_msg)
print('-- Resetting position')
await drone.offboard.set_position_ned(
PositionNedYaw(reset_pos[0], reset_pos[1], -reset_pos[2],
reset_pos[3]))
while True:
await asyncio.sleep(0.1)
# ob = [lin_pos[2], lin_vel[2]]
# if ( ( np.abs(ob[0] + reset_pos[2]) < 0.5 ) and np.abs( ob[1] < 0.05 ) ):
# await asyncio.sleep(1)
# break
if np.abs(np.linalg.norm(lin_pos - reset_pos[0:3])) < 0.5 and np.abs(
np.linalg.norm(lin_vel)) < 0.05:
await asyncio.sleep(1)
break
print('-- Position reset')
pause_msg = WorldControl()
pause_msg.pause = True
await asyncio.sleep(0.001)
await pub_world_control.publish(pause_msg)
def control_loop(driver, time_out, loop):
manager = yield From(pygazebo.connect())
yield From(trollius.sleep(0.5))
world_subscriber = manager.subscribe('/gazebo/default/world_stats',
'gazebo.msgs.WorldStatistics',
world_callback)
location_subscriber = manager.subscribe('/gazebo/default/pose/info',
'gazebo.msgs.PosesStamped',
location_callback)
light_sensor1_subscriber = manager.subscribe(
'/gazebo/default/husky/camera1/link/camera/image',
'gazebo.msgs.ImageStamped', light_1_callback)
light_sensor2_subscriber = manager.subscribe(
'/gazebo/default/husky/camera2/link/camera/image',
'gazebo.msgs.ImageStamped', light_2_callback)
laser_subscriber = manager.subscribe(
'/gazebo/default/husky/hokuyo/link/laser/scan',
'gazebo.msgs.LaserScanStamped', laser_callback)
yield From(trollius.sleep(0.5))
world_publisher = yield From(
manager.advertise('/gazebo/default/world_control',
'gazebo.msgs.WorldControl'))
world_publisher.wait_for_listener()
wheel_publisher = yield From(
manager.advertise('/gazebo/default/husky/joint_cmd',
'gazebo.msgs.JointCmd'))
wheel_publisher.wait_for_listener()
world_control = WorldControl()
world_control.pause = True
world_control.reset.all = True
yield From(trollius.sleep(0.01))
yield From(world_publisher.publish(world_control))
left_wheel = JointCmd()
left_wheel.name = 'husky::front_left_joint'
right_wheel = JointCmd()
right_wheel.name = 'husky::front_right_joint'
left_wheel.velocity.target = 0
right_wheel.velocity.target = 0
yield From(trollius.sleep(0.01))
yield From(wheel_publisher.publish(left_wheel))
yield From(wheel_publisher.publish(right_wheel))
world_control.pause = False
yield From(trollius.sleep(0.01))
yield From(world_publisher.publish(world_control))
yield From(trollius.sleep(0.01))
global sim_time
start_time = sim_time
end_time = start_time + time_out
global distance_to_goal
global collide
global light_sensor1
global light_sensor2
print "Starting control loop"
while (sim_time < end_time) and (distance_to_goal > 0.5):
sensor_input = [light_sensor1, light_sensor2]
sensor_input += collide
(left, right) = driver(sensor_input)
left_wheel.velocity.target = left
right_wheel.velocity.target = right
yield From(wheel_publisher.publish(left_wheel))
yield From(wheel_publisher.publish(right_wheel))
update_distance()
if distance_to_goal < 0.5:
break
yield From(trollius.sleep(.01))
yield From(trollius.sleep(0.01))
world_control.pause = True
yield From(world_publisher.publish(world_control))
global left_velocity
global right_velocity
left_velocity = left_wheel.velocity.target
right_velocity = right_wheel.velocity.target
loop.stop()