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 _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 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)