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)
