def open(self): rospy.logwarn_once( 'The Tornado Rosbridge WebSocket implementation is deprecated.' ' See rosbridge_server.autobahn_websocket' ' and rosbridge_websocket.py') cls = self.__class__ parameters = { "fragment_timeout": cls.fragment_timeout, "delay_between_messages": cls.delay_between_messages, "max_message_size": cls.max_message_size, "unregister_timeout": cls.unregister_timeout, "bson_only_mode": cls.bson_only_mode } try: self.protocol = RosbridgeProtocol(cls.client_id_seed, parameters=parameters) self.protocol.outgoing = self.send_message self.set_nodelay(True) self.authenticated = False self._write_lock = threading.RLock() cls.client_id_seed += 1 cls.clients_connected += 1 self.client_id = uuid.uuid4() if cls.client_manager: cls.client_manager.add_client(self.client_id, self.request.remote_ip) except Exception as exc: rospy.logerr("Unable to accept incoming connection. Reason: %s", str(exc)) rospy.loginfo("Client connected. %d clients total.", cls.clients_connected) if cls.authenticate: rospy.loginfo("Awaiting proper authentication...")
def gait_selection_callback(self, msg): new_gait_type = msg.gait_type if (new_gait_type is None or new_gait_type == "" or not rospy.has_param("~gait_types/{gait_type}".format( gait_type=new_gait_type))): rospy.logwarn_once( "The gait has unknown gait type of `{gait_type}`, default is set to walk_like" .format(gait_type=new_gait_type)) new_gait_type = "walk_like" self._gait_type = new_gait_type self.load_current_gains() needed_gains = [ self.look_up_table(i) for i in range(len(self._joint_list)) ] if not self.done_interpolation_test(needed_gains): rate = rospy.Rate(10) rospy.loginfo( "Beginning PID interpolation for gait type: {0}".format( self._gait_type)) begin_time = rospy.get_time() self._last_update_times = len(self._joint_list) * [begin_time] while not self.done_interpolation_test(needed_gains): self.client_update(needed_gains) rate.sleep() rospy.loginfo( "PID interpolation finished in {0}s".format(rospy.get_time() - begin_time))
def excecute(): global vel_pub global steer_max global steer_min global drive_max global drive_min # ROS initialize rospy.init_node('adrv_remote') rate = rospy.Rate(30) joy_sub = rospy.Subscriber('joy', Joy, joystickSubscribe, queue_size=10) vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10) while not rospy.is_shutdown(): try: steer_max = rospy.get_param('~steer_max') steer_min = rospy.get_param('~steer_min') drive_max = rospy.get_param('~drive_max') drive_min = rospy.get_param('~drive_min') except KeyError as ex: rospy.logwarn_once(ex + "not found parameter") rate.sleep()
def run(self): while not rospy.is_shutdown(): if self._is_laser_scan_publish_asyn: with self._laser_scan_cache_lock: len_cache = len(self._laser_scan_cache) # if no cache, do nothing # if cache size == 2 , laser scan pubish rate too slow compared to coming data if len_cache == 0: continue else: latest_laser_scan = self._laser_scan_cache[-1] self._pub_laser_scan.publish(latest_laser_scan) # it means the the interact rate is too high, some of the data will be discarded if len_cache == 3 and rospy.Time.now().to_sec( ) - self.start_time.to_sec() > 10: interact_rate = 2*1 / \ (self._laser_scan_cache[-1].header.stamp.to_sec() - self._laser_scan_cache[0].header.stamp.to_sec()) rospy.logwarn_once( "The rate [{:3.1f} FPS] of republishment of the laser scan is lower compared to the \ receivings [approximately {:3.1f} FPS], therefore some the them are discareded" .format(self._laser_scan_pub_rate, interact_rate)) # chane the cache size to 1 while len(self._laser_scan_cache) != 1: self._laser_scan_cache.popleft() self._laser_scan_pub_rater.sleep() else: rospy.spin()
def set_occupancy_grid(self, req): if not self.is_map_loaded: self._map.from_msg(req.map) return True else: rospy.logwarn_once("Map not set. Already loaded one.") return False
def set_goal(self): ''' Sets next waypoint fot the bot to travel to ''' if self.final_goal is not None: try: if np.sqrt((self.final_goal.x - self.position.x)**2 + (self.position.y - self.final_goal.y)**2) < 0.1: rospy.loginfo_once('final_goal_reached') self.goal_reached = True self.move_to_goal = False self.current_index = 0 if len(self.path_points) != 0 and not self.goal_reached: self.nextWay = self.path_points[self.current_index] # print(self.current_index) if abs(self.nextWay.x - self.position.x) < DISTMIN and abs(self.nextWay.y - self.position.y) < DISTMIN: self.move_to_goal = False self.current_index += 1 else : self.goal = self.nextWay self.move_to_goal = True self._move_bot() elif self.goal_reached: self.velocity.linear.x, self.velocity.linear.y = 0, 0 self.vel_pub.publish(self.velocity) rospy.loginfo_once('-------Completed Path-------') rospy.loginfo_once('-------Waiting for new goal------') except Exception as err: rospy.logwarn_once(err) self.vel_pub.publish(Twist())
def computeTrajectoryAdjustment(self): # Only consider objects in a -60 to 60 degree cone # (60 deg = approx 1 rad) if self.laser_msg.angle_increment == 0: rospy.logwarn_once("No valid lidar data") return 0 # Lidar is mounted backwards, so "rotate" ranges ranges = self.laser_msg.ranges[180:] + self.laser_msg.ranges[:181] start_idx = \ int((-1-self.laser_msg.angle_min)/self.laser_msg.angle_increment) end_idx = len(ranges) - \ int((self.laser_msg.angle_max-1)/self.laser_msg.angle_increment) heading_adjustment = 0 for idx in range(start_idx, end_idx): angle = self.laser_msg.angle_min + \ idx*self.laser_msg.angle_increment distance = ranges[idx] # Avoid divide by zero error if abs(angle) < 0.001: continue if not np.isinf(distance): adj = (1 / distance) * 3 * (1 / angle) heading_adjustment += adj return heading_adjustment
def advertise( self, *args, **kwargs ): rospy.logwarn_once("[NodeLazy] This node subscribes topics only when subscribed.") if self.subscribe_listener is None: self.subscribe_listener = self.SubscribeListener( self.subscribe, self.unsubscribe ) kwargs['subscriber_listener'] = self.subscribe_listener return rospy.Publisher( *args, **kwargs )
def wait_for_internet_connection(self): # This is to prevent the node to crash if there is no internet connection rate = rospy.Rate(0.1) while not self.test_internet_connection(): rospy.logwarn_once( 'MQTT Client - Waiting for an internet connection, re-checking every 10s...' ) rospy.logdebug( 'MQTT Client - Waiting for an internet connection, re-checking in 10s...' ) rate.sleep()
def __set_do(self, msg): # type: (String) -> None if not self.robot_interface.is_connected(): self.robot_interface.reconnect() try: do_index, do_value = msg.data.split(" ") self.robot_interface.set_io_value(int(do_index), bool(int(do_value) == 1)) except: rospy.logwarn_once( "Set DO Message format is wrong, expected 'DO_INDEX DO_VALUE' (e.g. '12 1') but got {}" .format(msg.data))
def decode(self, sentence): tag = sentence[0] if tag == self.gga: self.decode_gga(sentence) elif tag == self.gst: self.decode_gst(sentence) elif tag == self.vtg: self.decode_vtg(sentence) else: rospy.logwarn_once( 'Warning [ZEDF9P.decode]: Receiving sentences that are not recognized' )
def onVU8Update(self, msg): inMsg = msg outMsg = inMsg # outMsg.header = inMsg.header # outMsg.scan_time = inMsg.scan_time # outMsg.angle_increment = inMsg.angle_increment # outMsg.angle_max = inMsg.angle_max # outMsg.angle_min = inMsg.angle_min LiDAR_FOV = degrees(inMsg.angle_max - inMsg.angle_min) LiDAR_segInc = degrees(inMsg.angle_increment) inputSamples = len(inMsg.ranges) samplePerSeg = inputSamples / self.VU8Segments outMsg.angle_increment = inMsg.angle_increment * samplePerSeg # print("Input samples:", inputSamples) # print("LidarRange:", int(LiDAR_FOV)) # print("Samples per segment: %d, degrees perSample %.2f" % # (samplePerSeg, LiDAR_segInc)) vu8Ranges = [] vu8Amplitudes = [] for seg in range(0, self.VU8Segments): nearestDistRng = 99999999.0 nearestDistAmp = 0.0 for i in range(0, samplePerSeg): # print("Sample: ",i+seg*samplePerSeg) currSampleRng = inMsg.ranges[i + seg * samplePerSeg] currSampleAmp = inMsg.intensities[i + seg * samplePerSeg] if not isinf(currSampleRng): if currSampleRng < nearestDistRng: nearestDistRng = currSampleRng nearestDistAmp = currSampleAmp if nearestDistRng > 99999998.0: nearestDistRng = float('Inf') vu8Ranges.append(nearestDistRng) vu8Amplitudes.append(nearestDistAmp) outMsg.ranges = vu8Ranges outMsg.intensities = vu8Amplitudes if int(LiDAR_FOV) == 100: self.VU8_100_Pub.publish(outMsg) elif int(LiDAR_FOV) == 48: self.VU8_48_Pub.publish(outMsg) else: msg = ("simLeddar - sensor neither 48 or 100 degrees. FOV: %.2f" % LiDAR_FOV) rospy.logwarn_once(msg) pass
def get_pressure_filtered_right(self): if len(self.simulation.pressure_sensors) == 0: rospy.logwarn_once("No pressure sensors found in simulation model") return self.foot_msg_right f_rlb = self.simulation.pressure_sensors["RLB"].get_force() f_rlf = self.simulation.pressure_sensors["RLF"].get_force() f_rrf = self.simulation.pressure_sensors["RRF"].get_force() f_rrb = self.simulation.pressure_sensors["RRB"].get_force() self.foot_msg_right.left_back = f_rlb[1] self.foot_msg_right.left_front = f_rlf[1] self.foot_msg_right.right_front = f_rrf[1] self.foot_msg_right.right_back = f_rrb[1] return self.foot_msg_right
def __init__(self): rospy.init_node("perception_manager", anonymous=True) self.curr_time = rospy.get_time() self.prev_time = float('inf') rospy.wait_for_service('perception_service') try: self.perception_service = rospy.ServiceProxy( 'perception_service', SetBool) except rospy.ServiceException as e: rospy.logwarn_once("Service call failed: %s" % e)
def wait_for_credentials_creation(self): # We NEED to wait for the credentials since the first time the robot will # start it won't have them but will still need to communicate with AWS later rate = rospy.Rate(0.1) while not os.path.isfile(self.__ca_file) or not os.path.isfile(self.__cert_file) \ or not os.path.isfile(self.__key_file): rospy.logwarn_once( 'MQTT Client - Waiting for the certificates, re-checking every 10s...' ) rospy.logdebug( 'MQTT Client - Waiting for the certificates, re-checking in 10s...' ) rate.sleep()
def send_perception_concept_to_d2c(self): if self.client_perception_concept_decriptor: m = srv.PerceptionConceptDescriptorRequest() m.device_name = self.name m.descriptor_json = dict_to_json_str(self.p_concept_json) try: r = self.client_perception_concept_decriptor(m) except ServiceException: logerr("Service PerceptionConceptDescriptor error!") r = None else: logwarn_once("The function was turned off!") r = None return r
def __init__(self): utm_zone = rospy.get_param('~utm_zone', default=17) bags = glob.glob('*.bag') print 'Bag files found: ' + str(len(bags)) for fname in bags: print ' ' + fname plotter_service_exists = True for bag_path in bags: if rospy.is_shutdown(): break gm_plotter = GmPlotter(utm_zone) rospy.loginfo('Processing bag file [%s]' % bag_path) try: bag = rosbag.Bag(bag_path) file_name = bag_path.split('.')[0] gps_topics = [] topic_info = bag.get_type_and_topic_info() for i in topic_info.topics: if topic_info.topics[i][0] == 'nav_msgs/Odometry' or topic_info.topics[i][0] == 'sensor_msgs/NavSatFix': topics.append(i) for _, msg, _ in bag.read_messages(topics=gps_topics): if msg._type == 'sensor_msgs/NavSatFix': gm_plotter.add_point(msg.latitude, msg.longitude) elif msg._type == 'nav_msgs/Odometry': gm_plotter.add_utm_point(msg.pose.pose.position.x, msg.pose.pose.position.y) else: rospy.logwarn_once('Unsupported message type: %s' % msg._type) if rospy.is_shutdown(): break if plotter_service_exists: plotter_service_exists = gm_plotter.make_plot(os.path.abspath(os.curdir) + '/' + file_name + '.html') rospy.sleep(2.0) bag.close() except IOError, e: rospy.logerr(str(e)) except OSError, e: rospy.logerr(str(e))
def get_pressure_filtered_left(self): if len(self.simulation.pressure_sensors) == 0: rospy.logwarn_once("No pressure sensors found in simulation model") return self.foot_msg_left f_llb = self.simulation.pressure_sensors[ self.robot_index]["LLB"].get_force() f_llf = self.simulation.pressure_sensors[ self.robot_index]["LLF"].get_force() f_lrf = self.simulation.pressure_sensors[ self.robot_index]["LRF"].get_force() f_lrb = self.simulation.pressure_sensors[ self.robot_index]["LRB"].get_force() self.foot_msg_left.left_back = f_llb[1] self.foot_msg_left.left_front = f_llf[1] self.foot_msg_left.right_front = f_lrf[1] self.foot_msg_left.right_back = f_lrb[1] return self.foot_msg_left
def on_cmd_raw(msg): left = msg.left right = msg.right if left > MAX_SPEED or left < -MAX_SPEED or right > MAX_SPEED or right < -MAX_SPEED: rospy.logwarn_once( 'specified speed out of range, must be between {} and {}'.format( -MAX_SPEED, MAX_SPEED)) left = min(max(-MAX_SPEED, left), MAX_SPEED) right = min(max(-MAX_SPEED, right), MAX_SPEED) rospy.loginfo('setting speed to {:5.3f}, {:5.3f}'.format(left, right)) # TODO: somehow I got the polarity reversed on the motors if abs(left) < .05: left = 0.0 if abs(right) < .05: right = 0.0 set_speed(motor_left_ID, -left) set_speed(motor_right_ID, -right)
def send_emotion_core_data_descriptor(self): if self.client_emotion_core_data_descriptor: m = srv.EmotionCoreDataDescriptorRequest() data_type = self.data_dsc_json["data_type"] m.sensor_name = f"{self.name}:{data_type}" m.val_max = self.data_dsc_json["value_max"] m.val_min = self.data_dsc_json["value_min"] m.weights_json = dict_to_json_str(self.data_dsc_json["weights"]) try: r = self.client_emotion_core_data_descriptor(m) except ServiceException: logerr("Service EmotionCoreDataDescriptor error!") r = None else: logwarn_once("The function was turned off!") r = None return r
def merge(self): if (rospy.get_param(self._name + '/weight_hw_adjust') == 1): self.wei_irsa = self._joy_data[4:] / 2.0 else: parameters = rospy.get_param(dev_name) self.wei_irsa = np.array([ parameters['weight_idle'], parameters['weight_reflex'], parameters['weight_slave'], parameters['weight_auto'] ]) wei_irsa = self.wei_irsa "define the groups of joint irsa" joint_idle = sum(self.weights_norm[0] * self.payload_norm[0]) joint_reflex = sum(self.weights_norm[1] * self.payload_norm[1]) joint_slave = sum(self.weights_norm[2] * self.payload_norm[2]) joint_auto = sum(self.weights_norm[3] * self.payload_norm[3]) self.joint_means = np.array(wei_irsa[0]*joint_idle+ wei_irsa[1]*joint_reflex +\ wei_irsa[2]*joint_slave + wei_irsa[3]*joint_auto) "mask the joints" if (rospy.has_param(self._name + '/joint_mask_h') and rospy.has_param(self._name + '/joint_mask_l')): temp_mask = format(rospy.get_param(self._name + '/joint_mask_h'), '#0' + str(self._mhn + 2) + 'b') self._mask_h = np.array([int(m) for m in temp_mask[2:]]) temp_mask = format(rospy.get_param(self._name + '/joint_mask_l'), '#0' + str(self._mln + 2) + 'b') self._mask_l = np.array([int(m) for m in temp_mask[2:]]) else: rospy.logwarn_once( "Mixer: Some dynamic parameters cannot be set for some reasons. This would lead to a constant output." ) self._mask = np.concatenate((self._mask_h, self._mask_l), axis=0) self.joint_means = self._mask * self.joint_means "if interrupted, everytime it detected it is interrupted, add a clock to mention itself how long has passed" if (rospy.get_param(self._name + '/sys_interrupt') == 1): "clock start" self._clock += 1 return self.sys_interrupt(self._clock) else: self._clock = 0 return self.joint_means
def on_cmd_raw(msg): try: left = msg.left right = msg.right if left > MAX_SPEED or left < -MAX_SPEED or right > MAX_SPEED or right < -MAX_SPEED: rospy.logwarn_once( 'specified speed out of range, must be between {} and {}'. format(-MAX_SPEED, MAX_SPEED)) left = min(max(-MAX_SPEED, left), MAX_SPEED) right = min(max(-MAX_SPEED, right), MAX_SPEED) # TODO: somehow I got the polarity reversed on the motors if abs(left) < .05: left = 0.0 if abs(right) < .05: right = 0.0 set_speed(motor_left_ID, -left) set_speed(motor_right_ID, -right) except: rospy.logerr(traceback.format_exc())
def destroy(self): """ destroy all the players and sensors """ # destroy vehicles sensors for actor_id in self.vehicles_sensors: destroy_object_request = DestroyObjectRequest(actor_id) try: response = self.destroy_object_service(destroy_object_request) except rospy.ServiceException as e: rospy.logwarn_once(str(e)) self.vehicles_sensors = [] # destroy global sensors for actor_id in self.global_sensors: destroy_object_request = DestroyObjectRequest(actor_id) try: response = self.destroy_object_service(destroy_object_request) except rospy.ServiceException as e: rospy.logwarn_once(str(e)) self.global_sensors = [] # destroy player for player_id in self.players: destroy_object_request = DestroyObjectRequest(player_id) try: self.destroy_object_service(destroy_object_request) except rospy.ServiceException as e: rospy.logwarn_once(str(e)) self.players = []
def update(self): data = self.bb.get(bb_enums.GPS_FIX) if (data is None or data.latitude is None or data.latitude == 0.0 or data.longitude is None or data.latitude == 0.0 or data.status.status == -1): rospy.loginfo_throttle_identical( self._spam_period, "GPS lat/lon are 0s or Nones, cant set utm zone/band from these >:( " ) # shitty gps self._spam_period = min(self._spam_period * 2, self._max_spam_period) return pt.Status.SUCCESS self.gps_zone, self.gps_band = fromLatLong(data.latitude, data.longitude).gridZone() if self.gps_zone is None or self.gps_band is None: rospy.logwarn_throttle_identical( 10, "gps zone and band from fromLatLong was None") return pt.Status.SUCCESS # first read the UTMs given by ros params prev_band = self.bb.get(bb_enums.UTM_BAND) prev_zone = self.bb.get(bb_enums.UTM_ZONE) if prev_zone != self.gps_zone or prev_band != self.gps_band: rospy.logwarn_once( "PREVIOUS UTM AND GPS_FIX UTM ARE DIFFERENT!\n Prev:" + str((prev_zone, prev_band)) + " gps:" + str((self.gps_zone, self.gps_band))) if common_globals.TRUST_GPS: rospy.logwarn_once("USING GPS UTM!") self.bb.set(bb_enums.UTM_ZONE, self.gps_zone) self.bb.set(bb_enums.UTM_BAND, self.gps_band) else: rospy.logwarn_once("USING PREVIOUS UTM!") self.bb.set(bb_enums.UTM_ZONE, prev_zone) self.bb.set(bb_enums.UTM_BAND, prev_band) return pt.Status.SUCCESS
def __init__(self): rospy.init_node('deepSpaceNode', anonymous=False) markerPub = rospy.Publisher('space_marker_topic', Marker, queue_size=10) hitPub = rospy.Publisher('final_frontier_hit_topic', Bool, queue_size=10) listener = tf.TransformListener() # parameters space_frame_id = rospy.get_param('~space_frame_id', 'not_so_deep_space') rocket_frame_id = rospy.get_param('~rocket_frame_id', 'rocket') border_dist = 50 rate = rospy.Rate(1) self.planeMarker = Marker() self.planeMarker.header.frame_id = space_frame_id self.planeMarker.header.stamp = rospy.get_rostime() self.planeMarker.ns = "space_markers" self.planeMarker.id = 1 self.planeMarker.type = 1 # cube self.planeMarker.action = 0 self.planeMarker.pose.position.x = 0 self.planeMarker.pose.position.y = 0 self.planeMarker.pose.position.z = 0 self.planeMarker.pose.orientation.x = 0 self.planeMarker.pose.orientation.y = 0 self.planeMarker.pose.orientation.z = 0 self.planeMarker.pose.orientation.w = 1.0 self.planeMarker.scale.x = border_dist * 2. self.planeMarker.scale.y = border_dist * 2. self.planeMarker.scale.z = 0.1 self.planeMarker.color.r = 0.0 self.planeMarker.color.g = 1.0 self.planeMarker.color.b = 0.0 self.planeMarker.color.a = 1.0 self.planeMarker.lifetime = rospy.Duration(0.0) self.planeMarker.frame_locked = True self.border1Marker = Marker() self.border1Marker.header.frame_id = space_frame_id self.border1Marker.header.stamp = rospy.get_rostime() self.border1Marker.ns = "space_markers" self.border1Marker.id = 2 self.border1Marker.type = 1 # cube self.border1Marker.action = 0 self.border1Marker.pose.position.x = 0 self.border1Marker.pose.position.y = border_dist self.border1Marker.pose.position.z = 2 self.border1Marker.pose.orientation.x = 0 self.border1Marker.pose.orientation.y = 0 self.border1Marker.pose.orientation.z = 0 self.border1Marker.pose.orientation.w = 1.0 self.border1Marker.scale.x = border_dist * 2. self.border1Marker.scale.y = 0.1 self.border1Marker.scale.z = 4 self.border1Marker.color.r = 1.0 self.border1Marker.color.g = 0.0 self.border1Marker.color.b = 0.0 self.border1Marker.color.a = 1.0 self.border1Marker.lifetime = rospy.Duration(0.0) self.border1Marker.frame_locked = True self.border2Marker = Marker() self.border2Marker.header.frame_id = space_frame_id self.border2Marker.header.stamp = rospy.get_rostime() self.border2Marker.ns = "space_markers" self.border2Marker.id = 3 self.border2Marker.type = 1 # cube self.border2Marker.action = 0 self.border2Marker.pose.position.x = 0 self.border2Marker.pose.position.y = -border_dist self.border2Marker.pose.position.z = 2 self.border2Marker.pose.orientation.x = 0 self.border2Marker.pose.orientation.y = 0 self.border2Marker.pose.orientation.z = 0 self.border2Marker.pose.orientation.w = 1.0 self.border2Marker.scale.x = border_dist * 2. self.border2Marker.scale.y = 0.1 self.border2Marker.scale.z = 4 self.border2Marker.color.r = 1.0 self.border2Marker.color.g = 0.0 self.border2Marker.color.b = 0.0 self.border2Marker.color.a = 1.0 self.border2Marker.lifetime = rospy.Duration(0.0) self.border2Marker.frame_locked = True self.border3Marker = Marker() self.border3Marker.header.frame_id = space_frame_id self.border3Marker.header.stamp = rospy.get_rostime() self.border3Marker.ns = "space_markers" self.border3Marker.id = 4 self.border3Marker.type = 1 # cube self.border3Marker.action = 0 self.border3Marker.pose.position.x = border_dist self.border3Marker.pose.position.y = 0 self.border3Marker.pose.position.z = 2 self.border3Marker.pose.orientation.x = 0 self.border3Marker.pose.orientation.y = 0 self.border3Marker.pose.orientation.z = 0 self.border3Marker.pose.orientation.w = 1.0 self.border3Marker.scale.x = 0.1 self.border3Marker.scale.y = border_dist * 2. self.border3Marker.scale.z = 4 self.border3Marker.color.r = 1.0 self.border3Marker.color.g = 0.0 self.border3Marker.color.b = 0.0 self.border3Marker.color.a = 1.0 self.border3Marker.lifetime = rospy.Duration(0.0) self.border3Marker.frame_locked = True self.border4Marker = Marker() self.border4Marker.header.frame_id = space_frame_id self.border4Marker.header.stamp = rospy.get_rostime() self.border4Marker.ns = "space_markers" self.border4Marker.id = 5 self.border4Marker.type = 1 # cube self.border4Marker.action = 0 self.border4Marker.pose.position.x = -border_dist self.border4Marker.pose.position.y = 0 self.border4Marker.pose.position.z = 2 self.border4Marker.pose.orientation.x = 0 self.border4Marker.pose.orientation.y = 0 self.border4Marker.pose.orientation.z = 0 self.border4Marker.pose.orientation.w = 1.0 self.border4Marker.scale.x = 0.1 self.border4Marker.scale.y = border_dist * 2. self.border4Marker.scale.z = 4 self.border4Marker.color.r = 1.0 self.border4Marker.color.g = 0.0 self.border4Marker.color.b = 0.0 self.border4Marker.color.a = 1.0 self.border4Marker.lifetime = rospy.Duration(0.0) self.border4Marker.frame_locked = True while not rospy.is_shutdown(): if(hitPub.get_num_connections() == 0): rospy.logwarn_once("message from space: nobody is listening!") else: rospy.loginfo_once("message from space: at least someone is around") try: (trans, _) = listener.lookupTransform( space_frame_id, rocket_frame_id, rospy.Time(0)) rocket_dist = np.abs(np.array(trans)) - border_dist if (rocket_dist > -8.).any(): hitPub.publish(True) rospy.loginfo_once("message from space: great, I see the TF between "+space_frame_id+" and "+rocket_frame_id+" now") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn_once("message from space: cannot find TF between "+space_frame_id+" and "+rocket_frame_id) continue self.planeMarker.header.stamp = rospy.get_rostime() self.border1Marker.header.stamp = rospy.get_rostime() self.border2Marker.header.stamp = rospy.get_rostime() self.border3Marker.header.stamp = rospy.get_rostime() self.border4Marker.header.stamp = rospy.get_rostime() #markerPub.publish(self.planeMarker) markerPub.publish(self.border1Marker) markerPub.publish(self.border2Marker) markerPub.publish(self.border3Marker) markerPub.publish(self.border4Marker) rate.sleep()
def on_cmd_dir(msg): rospy.loginfo(rospy.get_caller_id() + ' cmd_dir=%s', msg.data) rospy.logwarn_once('cmd_dir not implemented')
parm_init() rospy.logwarn("init_motor_parameters") #rospy.logwarn(last_joint_angle) while not rospy.is_shutdown(): leg_positions = notspot_robot.run() notspot_robot.change_controller() dx = notspot_robot.state.body_local_position[0] dy = notspot_robot.state.body_local_position[1] dz = notspot_robot.state.body_local_position[2] roll = notspot_robot.state.body_local_orientation[0] pitch = notspot_robot.state.body_local_orientation[1] yaw = notspot_robot.state.body_local_orientation[2] try: rospy.logwarn_once("hoge1") joint_angles = inverseKinematics.inverse_kinematics( leg_positions, dx, dy, dz, roll, pitch, yaw) for i in range(12): #rospy.logwarn("joint_angles[%d]=%f",i,(joint_angles[i])) LowCmd.motorCmd[i].q = joint_angles[i] publishers[i].publish(LowCmd.motorCmd[i]) except: pass rate.sleep()
def publish_foot_pressure(self): # some models dont have sensors if len(self.simulation.pressure_sensors) == 0: rospy.logwarn_once("No pressure sensors found in simulation model") return f_llb = self.simulation.pressure_sensors[ self.robot_index]["LLB"].get_force() f_llf = self.simulation.pressure_sensors[ self.robot_index]["LLF"].get_force() f_lrf = self.simulation.pressure_sensors[ self.robot_index]["LRF"].get_force() f_lrb = self.simulation.pressure_sensors[ self.robot_index]["LRB"].get_force() f_rlb = self.simulation.pressure_sensors[ self.robot_index]["RLB"].get_force() f_rlf = self.simulation.pressure_sensors[ self.robot_index]["RLF"].get_force() f_rrf = self.simulation.pressure_sensors[ self.robot_index]["RRF"].get_force() f_rrb = self.simulation.pressure_sensors[ self.robot_index]["RRB"].get_force() self.foot_msg_left.left_back = f_llb[0] self.foot_msg_left.left_front = f_llf[0] self.foot_msg_left.right_front = f_lrf[0] self.foot_msg_left.right_back = f_lrb[0] self.left_foot_pressure_publisher.publish(self.foot_msg_left) self.foot_msg_right.left_back = f_rlb[0] self.foot_msg_right.left_front = f_rlf[0] self.foot_msg_right.right_front = f_rrf[0] self.foot_msg_right.right_back = f_rrb[0] self.right_foot_pressure_publisher.publish(self.foot_msg_right) self.foot_msg_left.left_back = f_llb[1] self.foot_msg_left.left_front = f_llf[1] self.foot_msg_left.right_front = f_lrf[1] self.foot_msg_left.right_back = f_lrb[1] self.left_foot_pressure_publisher_filtered.publish(self.foot_msg_left) self.foot_msg_right.left_back = f_rlb[1] self.foot_msg_right.left_front = f_rlf[1] self.foot_msg_right.right_front = f_rrf[1] self.foot_msg_right.right_back = f_rrb[1] self.right_foot_pressure_publisher_filtered.publish( self.foot_msg_right) # center position on foot pos_x = 0.085 pos_y = 0.045 threshold = 0.0 cop_l = PointStamped() cop_l.header.frame_id = "l_sole" cop_l.header.stamp = rospy.Time.from_seconds(self.simulation.time) sum_of_forces = f_llb[1] + f_llf[1] + f_lrf[1] + f_lrb[1] if sum_of_forces > threshold: cop_l.point.x = (f_llf[1] + f_lrf[1] - f_llb[1] - f_lrb[1]) * pos_x / sum_of_forces cop_l.point.x = max(min(cop_l.point.x, pos_x), -pos_x) cop_l.point.y = (f_llf[1] + f_llb[1] - f_lrf[1] - f_lrb[1]) * pos_y / sum_of_forces cop_l.point.y = max(min(cop_l.point.y, pos_y), -pos_y) else: cop_l.point.x = 0 cop_l.point.y = 0 self.cop_l_pub_.publish(cop_l) cop_r = PointStamped() cop_r.header.frame_id = "r_sole" cop_r.header.stamp = rospy.Time.from_seconds(self.simulation.time) sum_of_forces = f_rlb[1] + f_rlf[1] + f_rrf[1] + f_rrb[1] if sum_of_forces > threshold: cop_r.point.x = (f_rlf[1] + f_rrf[1] - f_rlb[1] - f_rrb[1]) * pos_x / sum_of_forces cop_r.point.x = max(min(cop_r.point.x, pos_x), -pos_x) cop_r.point.y = (f_rlf[1] + f_rlb[1] - f_rrf[1] - f_rrb[1]) * pos_y / sum_of_forces cop_r.point.y = max(min(cop_r.point.y, pos_y), -pos_y) else: cop_r.point.x = 0 cop_r.point.y = 0 self.cop_r_pub_.publish(cop_r)
def writer(): pub = rospy.Publisher('/hummingbird/reward/', Rewards, queue_size=1) rospy.init_node('reward_publisher', anonymous=True) pub.publish() while not rospy.is_shutdown(): rospy.logwarn_once("Started the publisher")
def test_log(self): rosout_logger = logging.getLogger('rosout') import rospy self.assertTrue(len(rosout_logger.handlers) == 2) self.assertTrue(rosout_logger.handlers[0], rosgraph.roslogging.RosStreamHandler) self.assertTrue(rosout_logger.handlers[1], rospy.impl.rosout.RosOutHandler) default_ros_handler = rosout_logger.handlers[0] # Remap stdout for testing lout = StringIO() lerr = StringIO() test_ros_handler = rosgraph.roslogging.RosStreamHandler(colorize=False, stdout=lout, stderr=lerr) try: # hack to replace the stream handler with a debug version rosout_logger.removeHandler(default_ros_handler) rosout_logger.addHandler(test_ros_handler) import rospy rospy.loginfo("test 1") lout_last = lout.getvalue().splitlines()[-1] self.assert_("test 1" in lout_last) rospy.logwarn("test 2") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[WARN]" in lerr_last) self.assert_("test 2" in lerr_last) rospy.logerr("test 3") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[ERROR]" in lerr_last) self.assert_("test 3" in lerr_last) rospy.logfatal("test 4") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[FATAL]" in lerr_last) self.assert_("test 4" in lerr_last) # logXXX_once lout_len = -1 for i in range(3): rospy.loginfo_once("test 1") lout_last = lout.getvalue().splitlines()[-1] if i == 0: self.assert_("test 1" in lout_last) lout_len = len(lout.getvalue()) else: # making sure the length of lout doesnt change self.assert_(lout_len == len(lout.getvalue())) lerr_len = -1 for i in range(3): rospy.logwarn_once("test 2") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 2" in lerr_last) lerr_len = len(lerr.getvalue()) else: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) lerr_len = -1 for i in range(3): rospy.logerr_once("test 3") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 3" in lerr_last) lerr_len = len(lerr.getvalue()) else: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) lerr_len = -1 for i in range(3): rospy.logfatal_once("test 4") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 4" in lerr_last) lerr_len = len(lerr.getvalue()) else: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) # logXXX_throttle lout_len = -1 for i in range(3): rospy.loginfo_throttle(3, "test 1") lout_last = lout.getvalue().splitlines()[-1] if i == 0: self.assert_("test 1" in lout_last) lout_len = len(lout.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lout doesnt change self.assert_(lout_len == len(lout.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 1" in lout_last) lerr_len = -1 for i in range(3): rospy.logwarn_throttle(3, "test 2") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 2" in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 2" in lerr_last) lerr_len = -1 for i in range(3): rospy.logerr_throttle(3, "test 3") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 3" in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 3" in lerr_last) lerr_len = -1 for i in range(3): rospy.logfatal_throttle(3, "test 4") lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: self.assert_("test 4" in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(1)) elif i == 1: # making sure the length of lerr doesnt change self.assert_(lerr_len == len(lerr.getvalue())) rospy.sleep(rospy.Duration(2)) else: self.assert_("test 4" in lerr_last) # logXXX_throttle_identical lout_len = -1 for i in range(5): if i < 2: test_text = "test 1" else: test_text = "test 1a" rospy.loginfo_throttle_identical(2, test_text) lout_last = lout.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lout_last) lout_len = len(lout.getvalue()) elif i == 1: # Confirm the length of lout hasn't changed self.assert_(lout_len == len(lout.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lout_last) lout_len = len(lout.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lout has changed self.assert_(lout_len != len(lout.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lout_last) lerr_len = -1 for i in range(5): if i < 2: test_text = "test 2" else: test_text = "test 2a" rospy.logwarn_throttle_identical(2, test_text) lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) elif i == 1: # Confirm the length of lerr hasn't changed self.assert_(lerr_len == len(lerr.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lerr has incremented self.assert_(lerr_len != len(lerr.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lerr_last) lerr_len = -1 for i in range(5): if i < 2: test_text = "test 3" else: test_text = "test 3a" rospy.logerr_throttle_identical(2, test_text) lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) elif i == 1: # Confirm the length of lerr hasn't changed self.assert_(lerr_len == len(lerr.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lerr has incremented self.assert_(lerr_len != len(lerr.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lerr_last) lerr_len = -1 for i in range(5): if i < 2: test_text = "test 4" else: test_text = "test 4a" rospy.logfatal_throttle_identical(2, test_text) lerr_last = lerr.getvalue().splitlines()[-1] if i == 0: # Confirm test text was logged self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) elif i == 1: # Confirm the length of lerr hasn't changed self.assert_(lerr_len == len(lerr.getvalue())) elif i == 2: # Confirm the new message was logged correctly self.assert_(test_text in lerr_last) lerr_len = len(lerr.getvalue()) rospy.sleep(rospy.Duration(2)) elif i == 3: # Confirm the length of lerr has incremented self.assert_(lerr_len != len(lerr.getvalue())) else: # Confirm new test text is in last log self.assert_(test_text in lerr_last) rospy.loginfo("test child logger 1", logger_name="log1") lout_last = lout.getvalue().splitlines()[-1] self.assert_("test child logger 1" in lout_last) rospy.logwarn("test child logger 2", logger_name="log2") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[WARN]" in lerr_last) self.assert_("test child logger 2" in lerr_last) rospy.logerr("test child logger 3", logger_name="log3") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[ERROR]" in lerr_last) self.assert_("test child logger 3" in lerr_last) rospy.logfatal("test child logger 4", logger_name="log4") lerr_last = lerr.getvalue().splitlines()[-1] self.assert_("[FATAL]" in lerr_last) self.assert_("test child logger 4" in lerr_last) finally: # restoring default ros handler rosout_logger.removeHandler(test_ros_handler) lout.close() lerr.close() rosout_logger.addHandler(default_ros_handler)