def start(self):
model_name = self.model_name
self.manager = yield From(pygazebo.connect())
self.publisher = yield From(self.manager.advertise(
'/gazebo/default/%s/joint_cmd' % model_name,
'gazebo.msgs.JointCmd'))
yield From(self.publisher.wait_for_listener())
self.subscriber = self.manager.subscribe(
'/gazebo/default/%s/joint_cmd' % model_name,
'gazebo.msgs.JointCmd',
self._receive_joint_cmd)
self.joint_cmd = joint_cmd_pb2.JointCmd()
self.joint_cmd.axis = 0
self.joint_cmd.position.target = 0
self.joint_cmd.position.p_gain = 10.0
self.joint_cmd.position.i_gain = 2.0
self.joint_cmd.position.d_gain = 0.2
self._servo_angles = {}
def subscribe_test():
# Nothing beyond the base fixture is required for this test.
manager = yield From(pygazebo.connect())
print('Pygazebo connected.')
received_data = []
first_data_future = trollius.Future()
def callback(data):
received_data.append(data)
if not first_data_future.done():
first_data_future.set_result(None)
listen = trollius.Future()
manager.server.read_packet(lambda data: listen.set_result(data))
subscriber = manager.manager.subscribe('subscribetopic', 'othermsgtype',
callback)
assert subscriber is not None
print('Before run listen')
loop.run_until_complete(listen)
print('After run listen')
packet_data = listen.result()
# We should have received a subscribe for this topic.
packet = packet_pb2.Packet.FromString(packet_data)
assert packet.type == 'subscribe'
subscribe = subscribe_pb2.Subscribe.FromString(packet.serialized_data)
assert subscribe.topic == 'subscribetopic'
assert subscribe.msg_type == 'othermsgtype'
def publish_loop():
manager = yield From(pygazebo.connect())
print("have manager")
publisher = yield From(
manager.advertise('/gazebo/default/stompy/joint_cmd',
'gazebo.msgs.JointCmd'))
print("have publisher")
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
#message.name = 'stompy::fl_leg::thigh_to_calf_upper'
#message.axis = 1
#message.force = -1000.0
#message.name = 'stompy::body_to_fl_leg'
#message.axis = 2
message.name = joint['name']
message.axis = joint['axis']
message.force = float(sys.argv[1])
#message.force = -1000
#message.position.p_gain = 10.0
#message.position.i_gain = 0.5
#message.position.target = 1.5707
#message.position.target = math.radians(float(sys.argv[1]))
print("message: %s" % message)
#message.name = 'stompy::fl_leg::hip_to_thigh'
#message.axis = 1
#message.force = -1000.0
for i in xrange(n):
print("Publish: %s" % i)
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
def sub_loop():
manager = yield from(pygazebo.connect())
sub_loop.is_waiting = True
def callback(data):
if not sub_loop.is_waiting:
return
message = pygazebo.msg.world_stats_pb2.WorldStatistics.fromString(data)
sim_time = message.sim_time
print('hi')
import math
sim_time = sim_time.sec + sim_time.nsec/math.pow(10, 9)
print(sim_time)
sub_loop.is_waiting = sim_time < target_time
subscriber = manager.subscribe('/gazebo/world/world_stats',
'gazebo.msgs.WorldStatistics',
callback)
yield from(subscriber.wait_for_connection())
while(sub_loop.is_waiting):
yield from(trollius.sleep(1.0))
#subscriber.remove() #TODO This feature in PyGazebo is bugged.
#Reimplement or hack around
#print('Subscriber removed')
print('Done waiting!')
def publish_loop(U):
manager = yield From(pygazebo.connect())
lift_publisher = yield From(
manager.advertise('/gazebo/default/trevor/front_gripper/lift_force',
'gazebo.msgs.JointCmd'))
grip_publisher = yield From(
manager.advertise('/gazebo/default/trevor/front_gripper/grip_force',
'gazebo.msgs.JointCmd'))
lift_message = pygazebo.msg.joint_cmd_pb2.JointCmd()
lift_message.axis = 0
lift_message.force = U.lift_force
grip_message = pygazebo.msg.joint_cmd_pb2.JointCmd()
grip_message.axis = 0
grip_message.force = U.grip_force
lift_message.name = "trevor::front_gripper::palm_raiser"
while True:
yield From(lift_publisher.publish(lift_message))
yield From(trollius.sleep(0.1))
grip_message.name = "trevor::front_gripper::left_finger_tip"
yield From(grip_publisher.publish(grip_message))
yield From(trollius.sleep(0.1))
grip_message.name = "trevor::front_gripper::right_finger_tip"
yield From(grip_publisher.publish(grip_message))
yield From(trollius.sleep(0.1))
grip_message.name = "trevor::front_gripper::palm_left_finger"
yield From(grip_publisher.publish(grip_message))
yield From(trollius.sleep(0.1))
grip_message.name = "trevor::front_gripper::palm_right_finger"
yield From(grip_publisher.publish(grip_message))
yield From(trollius.sleep(0.1))
print("Publishing")
def joint_force_loop():
manager = yield from (pygazebo.connect())
publisher = yield from (manager.advertise(
'/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
'gazebo.msgs.JointCmd'))
print("connected")
t_start = time.time()
t_end = time.time(
) + duration # The time that you want the controller to stop
while time.time() < t_end or duration == -1:
try:
#print(str(time.time()) + "waiting for: " + str(t_end))
for joint_name, force in zip(joint_names, forces):
yield from (publisher.wait_for_listener())
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
message.force = force
print(message.force)
yield from (publisher.publish(message))
yield from (trollius.sleep(1.0))
except Exception as e:
print("SOMETHING HAS GONE WRONG: " + str(e))
break
raise e
print("Connection closed")
def sub_loop():
manager = yield from (pygazebo.connect())
sub_loop.is_waiting = True
def callback(data):
if not sub_loop.is_waiting:
return
message = pygazebo.msg.world_stats_pb2.WorldStatistics.fromString(
data)
sim_time = message.sim_time
print('hi')
import math
sim_time = sim_time.sec + sim_time.nsec / math.pow(10, 9)
print(sim_time)
sub_loop.is_waiting = sim_time < target_time
subscriber = manager.subscribe('/gazebo/world/world_stats',
'gazebo.msgs.WorldStatistics', callback)
yield from (subscriber.wait_for_connection())
while (sub_loop.is_waiting):
yield from (trollius.sleep(1.0))
#subscriber.remove() #TODO This feature in PyGazebo is bugged.
#Reimplement or hack around
#print('Subscriber removed')
print('Done waiting!')
def publish_loop():
manager = yield From(pygazebo.connect())
print("have manager")
publisher = yield From(
manager.advertise('/gazebo/default/stompy/joint_cmd',
'gazebo.msgs.JointCmd'))
print("have publisher")
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
#message.name = 'stompy::fl_leg::thigh_to_calf_upper'
#message.axis = 1
#message.force = -1000.0
message.name = 'stompy::body_to_fl_leg'
message.axis = 2
message.force = 1000
#message.position.p_gain = 10.0
#message.position.i_gain = 0.5
#message.position.target = 1.5707
#message.name = 'stompy::fl_leg::hip_to_thigh'
#message.axis = 1
#message.force = -1000.0
print("have message")
while True:
yield From(publisher.publish(message))
print("published")
yield From(trollius.sleep(1.0))
print("sleeping")
def publish_loop():
manager = yield From(pygazebo.connect())
print("have manager")
publisher = yield From(
manager.advertise('/gazebo/default/stompy/joint_cmd',
'gazebo.msgs.JointCmd'))
print("have publisher")
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
#message.name = 'stompy::fl_leg::thigh_to_calf_upper'
#message.axis = 1
#message.force = -1000.0
#message.name = 'stompy::body_to_fl_leg'
#message.axis = 2
message.name = joint['name']
message.axis = joint['axis']
message.force = float(sys.argv[1])
#message.force = -1000
#message.position.p_gain = 10.0
#message.position.i_gain = 0.5
#message.position.target = 1.5707
#message.position.target = math.radians(float(sys.argv[1]))
print("message: %s" % message)
#message.name = 'stompy::fl_leg::hip_to_thigh'
#message.axis = 1
#message.force = -1000.0
for i in xrange(n):
print("Publish: %s" % i)
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
def _io_task_fn(self):
'''Runs until the simulation is done'''
self.stop_event = asyncio.Future()
try:
logger.info("Connecting to gazebo...")
retries = 5
while True:
try:
self.manager = yield from pygazebo.connect(address=(self._host, self._port))
except ConnectionRefusedError:
if retries == 0:
raise
logger.info("Error connecting to %s:%s, trying again (%s retries left)...", self._host, self._port, retries)
retries -= 1
yield from asyncio.sleep(1)
else:
break
except Exception as e:
self.start_event.set_exception(e)
else:
logger.info("Connected!")
self.start_event.set_result(None)
# Wait until done
yield from self.stop_event
logger.info("Gazebo communication thread is exiting")
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/mobile_base/joint_cmd',
'gazebo.msgs.JointCmd'))
# Python 2 limitation. In python 3 I'd define the variable here and use nonlocal
# If anyone thinks of a better way, let me know
global ave_speed_sp
global turn_angle_sp
def handleAveSpeedSp(new_speed):
global ave_speed_sp
ave_speed_sp = new_speed
print(new_speed)
def handleTurnAngleSp(new_angle):
global turn_angle_sp
turn_angle_sp = new_angle
print(new_angle)
def handleJointState(joint_state):
global left_wheel_rot, right_wheel_rot
global wheel_position_left, wheel_position_right
left_wheel_rot, right_wheel_rot = joint_state.velocity
wheel_position_left, wheel_position_right = joint_state.position
error_left_speed = left_wheel_rot * (wheel_diameter / 2) - ave_speed_sp
error_right_speed = right_wheel_rot * (wheel_diameter /
2) - ave_speed_sp
ul = error_left_speed * 1
ur = error_right_speed * 1
print('testing')
yield From(publisher.publish(LeftMessage(ul)))
yield From(publisher.publish(RightMessage(ur)))
rospy.Subscriber('/david/speed', Float32, handleAveSpeedSp)
rospy.Subscriber('/david/turn_angle', Float32, handleTurnAngleSp)
print(1)
manager.subscribe('/gazebo/default/mobile_base/joint_state',
'gazebo.msgs.JointState', handleJointState)
print(2)
# rospy.Subscriber('/joint_states', JointState, handleJointState)
# left_message = pygazebo.msg.joint_cmd_pb2.JointCmd()
# message.name = 'mobile_base::wheel_left_joint'
# message.axis = 0
# message.force = -0.0
while True:
pass
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()
def connect_loop():
global publisher
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/world_control',
'gazebo.msgs.WorldControl'))
def subscribe_loop():
manager = yield From(pygazebo.connect(address=('10.211.55.5', 11345)))
manager.subscribe('/gazebo/default/pose/info', 'gazebo.msgs.PosesStamped',
callback)
while True:
yield From(trollius.sleep(1))
def connect(future):
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/stompy/joint_cmd',
'gazebo.msgs.JointCmd'))
subscriber = yield From(
manager.subscribe('/gazebo/default/stompy/joint',
'gazebo.msgs.GzString', callback))
future.set_result(publisher)
def subscribe_loop():
manager = yield From(pygazebo.connect())
manager.start()
subscriber = manager.subscribe(TOPIC_NAME, TOPIC_MSG_TYPE, callback)
while True:
print("Listening")
print(subscriber)
yield From(trollius.sleep(1.0))
def subscribe_loop(address):
manager = yield From(pygazebo.connect(address=address))
manager.subscribe('/gazebo/default/pose/info',
'gazebo.msgs.PosesStamped',
parse_data)
while not EXIT:
yield From(trollius.sleep(0.2))
def publishMessageLoop(self, message):
if not self.connected:
self.manager = yield From(pygazebo.connect())
self.publisher = yield From(
self.manager.advertise(self.topic_name, self.message_type))
yield From(trollius.sleep(0.5))
pub_res = self.publisher.publish(message)
res = yield From(pub_res)
self.connected = True
def start():
print('what')
model_name = coxa_hinge0
manager = yield From(pygazebo.connect())
subscriber = manager.subscribe(TOPIC_NAME % model_name, TOPIC_MSG_TYPE, callback)
print('2')
while True:
print('loop')
eventlet.sleep(1)
def publish_loop():
manager = yield From(pygazebo.connect())
manager.subscribe('/gazebo/default/pose/info',
'gazebo.msgs.PosesStamped',
callback)
while True:
# yield From(subscriber.)
yield From(trollius.sleep(1.0))
def publish_loop():
manager = yield From(pygazebo.connect(('localhost', 11345)))
publisher = yield From(
manager.advertise('~/my_velodyne/vel_cmd', 'gazebo.msgs.vel_cmd'))
message = pygazebo.msg.vector3d_pb2.Vector3d()
message.z = -100
message.x = 200
message.y = 300
while True:
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
def connect(future):
manager = yield From(
pygazebo.connect())
publisher = yield From(
manager.advertise(
'/gazebo/default/stompy/joint_cmd',
'gazebo.msgs.JointCmd'))
subscriber = yield From(
manager.subscribe(
'/gazebo/default/stompy/joint',
'gazebo.msgs.GzString',
callback))
future.set_result(publisher)
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/trevor/model/skid_drive/vel_cmd',
'gazebo.msgs.Int'))
message = pygazebo.msg.int_pb2.Int()
message.data = self.rotate_angle
while running:
yield From(publisher.publish(message))
yield From(trollius.sleep(0.1))
def blockbar_action():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/vrc_task_1/blockingbar',
'gazebo.msgs.GzString'))
message = pygazebo.msg.gz_string_pb2.GzString()
message.data = "-1"
while True:
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
print message
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/simple_arm_0/joint_cmd',
'gazebo.msgs.JointCmd'))
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = 'simple_arm_0::arm_shoulder_pan_joint'
message.position.target = 3
yield From(publisher.publish(message))
yield From(trollius.sleep(0.5))
yield From(publisher.publish(message))
yield From(trollius.sleep(0.5))
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/model/joint_cmd',
'gazebo.msgs.JointCmd'))
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = 'robot::joint_name'
message.axis = 0
message.force = 1.0
while True:
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
def publish_loop(U):
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/trevor/cmd_vel',
'gazebo.msgs.Pose'))
message = pygazebo.msg.pose_pb2.Pose()
message.position.x = U.translate_speed
message.orientation.z = rpy_to_quaternion(
Rpy(0, 0, (degree_to_rad(U.rotate_speed)))).z
while True:
yield From(publisher.publish(message))
yield From(trollius.sleep(0.1))
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/trevor/joint_cmd',
'gazebo.msgs.JointCmd'))
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = 'trevor::front_gripper::palm_riser'
message.axis = 0
message.force = 10.0
count = 0
while True:
print("Publishing message #", count)
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
count += 1
def coroutine():
manager = yield from(pygazebo.connect())
print("connected")
publisher = yield from(
manager.advertise("/gazebo/" + world_name + "/world_control",
'gazebo.msgs.WorldControl'))
message = pygazebo.msg.world_control_pb2.WorldControl()
message.pause = pause
try:
yield from(publisher.wait_for_listener())
yield from(publisher.publish(message))
yield from(trollius.sleep(1.0))
except:
pass
print("Connection closed")
def coroutine():
manager = yield from (pygazebo.connect())
print("connected")
publisher = yield from (manager.advertise(
"/gazebo/" + world_name + "/world_control",
'gazebo.msgs.WorldControl'))
message = pygazebo.msg.world_control_pb2.WorldControl()
message.pause = pause
try:
yield from (publisher.wait_for_listener())
yield from (publisher.publish(message))
yield from (trollius.sleep(1.0))
except:
pass
print("Connection closed")
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/trevor/joint_cmd',
'gazebo.msgs.JointCmd'))
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = 'trevor::front_gripper::palm_riser'
message.axis = 0
message.force = 10.0
count = 0
while True:
print("Publishing message #", count)
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
count += 1
def subscribe_loop():
manager = yield From(pygazebo.connect())
#publisher = yield From(
# manager.advertise('/gazebo/default/topic',
# 'gazebo.msgs.GzString'))
yield From(trollius.sleep(1.0))
manager.start()
#manager.subscribe(
# '/gazebo/default/topic',
# 'gazebo.msgs.GzString',
# topic_callback)
manager.subscribe('/gazebo/default/stompy/joint', 'gazebo.msgs.Joint',
joint_callback)
while True:
#yield From(publisher.publish(
# pygazebo.msg.gz_string_pb2.GzString(data='hi')))
yield From(trollius.sleep(1.0))
def publish_loop():
manager = yield From(pygazebo.connect())
publisher = yield From(
manager.advertise('/gazebo/default/trevor/model/skid_drive/vel_cmd',
'gazebo.msgs.Pose'))
message = pygazebo.msg.pose_pb2.Pose()
message.position.x = self.translate_speed
Quaternion q
rpy_to_quaternion(q, 0, 0, self.rotate_speed)
message.orientation.x = q.x
message.orientation.y = q.y
message.orientation.z = q.z
message.orientation.w = q.w
while running:
yield From(publisher.publish(message))
yield From(trollius.sleep(0.1))
def joint_pose_loop(world_name, robot_name, joint_name, pose):
manager = yield from (pygazebo.connect())
publisher = yield from (manager.advertise(
'/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
'gazebo.msgs.JointCmd'))
print("connected")
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
#Message.position is a PID message with several values
message.position.target = pose
try:
yield from (publisher.wait_for_listener())
#This is needed to ensure that they are ready to publish.
yield from (publisher.publish(message))
except:
pass
print("Connection closed")
def joint_pose_loop(world_name, robot_name, joint_name, pose):
manager = yield from(pygazebo.connect())
publisher = yield from(
manager.advertise('/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
'gazebo.msgs.JointCmd'))
print("connected")
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
#Message.position is a PID message with several values
message.position.target = pose
try:
yield from(publisher.wait_for_listener())
#This is needed to ensure that they are ready to publish.
yield from(publisher.publish(message))
except:
pass
print("Connection closed")
def publish_loop():
print("gets to 1")
manager = yield From(pygazebo.connect(('localhost', 11345)))
# publisher = yield From(
# manager.advertise('/gazebo/default/model/joint_cmd',
# 'gazebo.msgs.JointCmd'))
publisher = yield From(
manager.advertise('~/my_velodyne/vel_cmd', 'gazebo.msgs.vel_cmd'))
print("gets to 2")
# message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message = pygazebo.msg.vector3d_pb2.Vector3d()
# message.name = 'robot::left_wheel_hinge'
# message.axis = 0
# message.force = 1000.0
message.z = -100
message.x = 200
message.y = 300
print("gets to 3")
while True:
yield From(publisher.publish(message))
yield From(trollius.sleep(1.0))
print("gets to 4")
def start(self):
model_name = self.model_name
self.manager = yield From(pygazebo.connect())
self.publisher = yield From(
self.manager.advertise("/gazebo/default/%s/joint_cmd" % model_name, "gazebo.msgs.JointCmd")
)
yield From(self.publisher.wait_for_listener())
self.subscriber = self.manager.subscribe(
"/gazebo/default/%s/joint_cmd" % model_name, "gazebo.msgs.JointCmd", self._receive_joint_cmd
)
self.joint_cmd = joint_cmd_pb2.JointCmd()
self.joint_cmd.axis = 0
self.joint_cmd.position.target = 0
self.joint_cmd.position.p_gain = 10.0
self.joint_cmd.position.i_gain = 2.0
self.joint_cmd.position.d_gain = 0.2
self._servo_angles = {}
def connect(self):
connected = False
for i in range(self.timeout):
try:
self.manager = yield From(
pygazebo.connect(('localhost', 11345)))
connected = True
break
except Exception as e:
pass
yield From(asyncio.sleep(1))
if connected:
self.poses_subscriber = self.manager.subscribe(
'/gazebo/default/pose/info', 'gazebo.msgs.PosesStamped',
self.poses_callback)
yield From(self.poses_subscriber.wait_for_connection())
self.running = True
while self.running:
yield From(asyncio.sleep(0.1))
else:
raise Exception("Timeout connecting to Gazebo.")
def publish_loop():
manager = yield From(pygazebo.connect())
# print('connected')
def callback(data):
global contacts_result
message = pygazebo.msg.contacts_pb2.Contacts.FromString(data)
if not message.contact: # no contact
# print('No Collision')
if not rospy.is_shutdown():
# rospy.loginfo(False)
pub.publish(False)
else: # have contact(s)
# print(len(message.contact))
dump_object(message)
# print contacts_result
_done = False
if len(contacts_result) > 0:
for i in contacts_result:
_done = _done or i
if not rospy.is_shutdown():
# rospy.loginfo(_done)
pub.publish(_done)
# empty the contacts list
contacts_result = []
subscriber = manager.subscribe('/gazebo/default/physics/contacts',
'gazebo.msgs.Contacts', callback)
while True:
yield From(subscriber.wait_for_connection())
yield From(trollius.sleep(0.01))
def _io_task_fn(self):
'''Runs until the simulation is done'''
self.stop_event = asyncio.Future()
try:
logger.info("Connecting to gazebo...")
retries = 5
while True:
try:
self.manager = yield from pygazebo.connect(
address=(self._host, self._port))
except ConnectionRefusedError:
if retries == 0:
raise
logger.info(
"Error connecting to %s:%s, trying again (%s retries left)...",
self._host, self._port, retries)
retries -= 1
yield from asyncio.sleep(1)
else:
break
except Exception as e:
self.start_event.set_exception(e)
else:
logger.info("Connected!")
self.start_event.set_result(None)
# Wait until done
yield from self.stop_event
logger.info("Gazebo communication thread is exiting")
def joint_force_loop():
manager = yield from(pygazebo.connect())
publisher = yield from(manager.advertise('/gazebo/' + world_name + '/' + robot_name + '/joint_cmd',
'gazebo.msgs.JointCmd'))
print("connected")
t_start = time.time()
t_end = time.time() + duration # The time that you want the controller to stop
while time.time() < t_end or duration == -1:
try:
#print(str(time.time()) + "waiting for: " + str(t_end))
for joint_name, force in zip(joint_names, forces):
yield from(publisher.wait_for_listener())
message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.name = robot_name + '::' + joint_name #format should be: name_of_robot + '::name_of_joint'
message.force = force
print(message.force)
yield from(publisher.publish(message))
yield from(trollius.sleep(1.0))
except Exception as e:
print("SOMETHING HAS GONE WRONG: " + str(e))
break
raise e
print("Connection closed")
def connect(address=default_address):
manager = yield From(pygazebo.connect(address=tuple(address)))
raise Return(manager)
