def generate_pose(T, camera_optical_frame):
print('publish TF for \n', T)
rate = rospy.Rate(30) # Hz
listener = tf.TransformListener()
try:
t, r = listener.lookupTransform('/base_link', camera_optical_frame,
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.error(
"failed to listen transform from '/base_link' to '/camera_optical_frame'"
)
continue
cam2base = numpy.matrix(tft.quaternion_matrix(r))
cam2base[0, 3] = t[0]
cam2base[1, 3] = t[1]
cam2base[2, 3] = t[2]
T = numpy.array(numpy.dot(cam2base, T))
p = Pose()
p.position.x = T[0, 3]
p.position.y = T[1, 3]
p.position.z = T[2, 3]
R = T[:3, :3]
roll, pitch, yaw = tft.euler_from_matrix(T)
q = tft.quaternion_from_euler(roll, pitch, yaw)
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
#tf_brocast(p, camera_optical_frame)
tf_brocast(p, "base_link")
def load_state(self, name):
name = "States.%s" % (name)
try:
__import__(name)
except ImportError, e:
rospy.error("Error importing state: %s" % (str(e)))
return None
def traffic_cb(self, msg):
if self.state != msg.state: # state change, game on
self.state = msg.state
b_clear = False
if msg.state == 0:
color = 'r'
elif msg.state == 1:
color = 'y'
elif msg.state == 2:
color = 'g'
elif msg.state == 4:
b_clear = True
else:
rospy.error('Unexpected traffic light status')
if not b_clear:
rospy.loginfo('Plot traffic light visual')
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
self.thisapp.plotTrafficLight(x, y, color)
else:
rospy.loginfo('Clearing traffic light visual')
self.thisapp.plotTrafficLightClear()
else:
pass
def load_state(self, name):
name = "States.%s" % (name)
try:
__import__(name)
except ImportError, e:
rospy.error("Error importing state: %s" % (str(e)))
return None
def main():
rospy.init_node('data_buffer')
# get path to store data
if rospy.has_param('data_buffer_path'):
data_buffer_path = rospy.get_param('data_buffer_path',
'/tmp/data_buffer')
if rospy.has_param('buffer_threshold'):
buffer_threshold = rospy.get_param('buffer_threshold', 10)
if rospy.has_param('test_image_topic'):
image_topic = rospy.get_param('test_image_topic')
else:
rospy.error('No image topic specified.')
if rospy.has_param('test_cmd_topic_stamped'):
cmd_topic = rospy.get_param('test_cmd_topic_stamped')
else:
rospy.error('No command topic specified.')
data_buffer = DataBuffer(data_buffer_path, buffer_threshold, image_topic,
cmd_topic)
rospy.loginfo("---------Params loaded----------")
rospy.loginfo("Buffer path: {}".format(data_buffer_path))
rospy.loginfo("Buffer threshold: {}".format(buffer_threshold))
rospy.loginfo("Image topic: {}".format(image_topic))
rospy.loginfo("Twist command topic: {}".format(cmd_topic))
rospy.spin()
def main():
rospy.init_node('util_timesync_publisher')
if rospy.has_param('test_cmd_topic'):
cmd_topic = rospy.get_param('test_cmd_topic')
else:
rospy.error('No command topic specified.')
TopicStamper(cmd_topic)
rospy.spin()
def _get_class(plugin_modules, class_name):
c = None
for module in plugin_modules:
if module in sys.modules:
if hasattr(sys.modules[module], class_name):
c = getattr(sys.modules[module], class_name)
else:
rospy.error("module {} is not found".format(module))
if c is None:
raise ValueError("class {} is not found in {}".format(
class_name, plugin_modules))
return c
def pose_cb(self, pose):
if self.waypoints is None:
rospy.error('No base_waypoints have been received by master')
return
current_pose = pose
# Compute the index of the waypoint closest to the current pose.
closest_wp_idx = helper.next_waypoint_index(current_pose, self.waypoints)
# Find number of waypoints ahead dictated by LOOKAHEAD_WPS
next_wps = self.waypoints[closest_wp_idx:closest_wp_idx + LOOKAHEAD_WPS]
self.publish(next_wps)
def confirm_speech(self):
if ConfirmRecognition.person == 'Unknown':
SpeechState.msg = (ARE_YOU_EXPECTED_MESSAGE + ' ' + YES_NO_MESSAGE)
elif ConfirmRecognition.person == 'Doctor':
SpeechState.msg = (ARE_YOU_MESSAGE + 'Doctor Kimble? ' + YES_NO_MESSAGE)
elif ConfirmRecognition.person == 'Deliman':
SpeechState.msg = (ARE_YOU_MESSAGE + 'the deli man? ' + YES_NO_MESSAGE)
elif ConfirmRecognition.person == 'Postman':
SpeechState.msg = (ARE_YOU_MESSAGE + 'the post man? ' + YES_NO_MESSAGE)
else:
rospy.error('Unknown person: ' + ConfirmRecognition.person)
SpeechState.next_state = 'CONFIRM_RECOGNITION'
def main():
rospy.init_node('predict_twist')
if rospy.has_param('test_image_topic'):
image_topic = rospy.get_param('test_image_topic')
else:
rospy.error('No image topic specified.')
if rospy.has_param('test_cmd_topic'):
cmd_topic = rospy.get_param('test_cmd_topic')
else:
rospy.error('No command topic specified.')
TwistPredictor(image_topic, cmd_topic)
rospy.spin()
def db_agg_cb(self, msg):
self._last_dashboard_message_time = rospy.get_time()
self._battery.set_power_state(msg.power_state)
if (msg.emergency_stop_state.emergency_state == 0):
self._runstop.set_ok()
self._runstop.setToolTip(
self.tr("Button stop: OK\nScanner stop: OK"))
else:
if msg.emergency_stop_state.emergency_button_stop:
self._runstop.set_button_stop()
elif msg.emergency_stop_state.scanner_stop:
self._runstop.set_scanner_stop()
else:
rospy.error("reason for emergency stop not known")
self._runstop.setToolTip(
self.tr("Button stop: %s\nScanner stop: %s" %
(str(msg.emergency_stop_state.emergency_button_stop),
str(msg.emergency_stop_state.scanner_stop))))
def reset(self, empty_msg):
# if self.controllerProcess is not None:
# self.controllerProcess.stop()
# self.controllerProcess = None
# package = 'baxter_interface'
# executable = 'joint_trajectory_action_server.py'
# node = roslaunch.core.Node(package, executable)
# launch = roslaunch.scriptapi.ROSLaunch()
# launch.start()
# self.controllerProcess = launch.launch(node)
if self.baxter is None:
rospy.error(
'BaxterSimulator: Baxter simulator is not set, did you call init()?'
)
self.baxter.stop()
# self.eStopPublisher.publish(data=True)
# self.eStopPublisher.publish(data=False)
# self._reloadControllers()
self._resetHardware()
self.baxter.reset()
self.baxter.enable()
rospy.loginfo('BaxterSimulator: Baxter successfully reset.')
return EmptyResponse()
def pose_cb(self, pose):
if self.waypoints is None:
rospy.error('No base_waypoints have been received by master')
return
if not self.dbw_enabled:
return
self.current_pose = pose
# Compute the index of the waypoint closest to the current pose.
closest_wp_idx = helper.next_waypoint_index_kdtree(
self.current_pose.pose, self.waypoints_kdtree)
# Find the closest stop line position if we have to stop the car.
_, stop_line_idx = self.stop_line_positions_kdtree.query(
[pose.pose.position.x, pose.pose.position.y])
stop_line_positions = self.stop_line_positions[stop_line_idx]
# Find the closest waypoint index for the stop line position
stop_line_pose = PoseStamped()
stop_line_pose.pose.position.x = stop_line_positions[0]
stop_line_pose.pose.position.y = stop_line_positions[1]
stop_line_pose.pose.orientation = pose.pose.orientation
stop_line_waypoint_idx = helper.next_waypoint_index_kdtree(
stop_line_pose.pose, self.waypoints_kdtree)
self.stop_line_waypoint_pub.publish(Int32(stop_line_waypoint_idx))
if closest_wp_idx == stop_line_waypoint_idx or self.current_velocity == 0.0:
rospy.logerr(
'Stop Line waypoint reached. Deceleration Sequence Stopped. Current Vel: %s',
self.current_velocity)
self.deceleration_started = False
self.deceleration_waypoints = None
# If the light is RED or YELLOW then slowly decrease the speed.
if self.current_traffic_light is not None:
rospy.logwarn(
'Light color: %s',
helper.get_traffic_light_color(
self.current_traffic_light.state))
if self.current_traffic_light.state == 0 or self.current_traffic_light.state == 1:
if helper.deceleration_rate(
self.current_velocity,
self.distance(self.waypoints, closest_wp_idx,
stop_line_waypoint_idx)) > 0.1:
if not self.deceleration_started:
rospy.logwarn('Deceleration Sequence Started')
new_waypoints = self.set_velocity_leading_to_stop_point(
closest_wp_idx, stop_line_waypoint_idx)
self.deceleration_started = True
self.deceleration_waypoints = new_waypoints
else:
rospy.logwarn(
'Using DECELERATION WAYPOINTS CALCULATED BEFORE')
new_waypoints = self.deceleration_waypoints
elif closest_wp_idx != stop_line_waypoint_idx:
# Simulate the behavior of a green light
new_waypoints = self.behavior_lights_green(closest_wp_idx)
else:
rospy.logerr(
'NOT PUBLISHING ANYTHING SINCE ASSUMING CAR IS AT STOP'
)
return # Do not publish anything. The car is at a STOP and we don't need to do anything
else:
# Lights are green
new_waypoints = self.behavior_lights_green(closest_wp_idx)
else:
new_waypoints = self.behavior_lights_green(closest_wp_idx)
if stop_line_waypoint_idx - closest_wp_idx < 5:
rospy.logerr('Current Waypoint: %s Current Velocity: %s',
closest_wp_idx, self.current_velocity)
for i in range(stop_line_waypoint_idx - 5,
stop_line_waypoint_idx + 1):
rospy.loginfo('%s, %s', i,
new_waypoints[i].twist.twist.linear.x)
# Find number of waypoints ahead dictated by LOOKAHEAD_WPS. However, if the car is stopped then don't accelerate
next_wps = new_waypoints[closest_wp_idx + 1:closest_wp_idx +
LOOKAHEAD_WPS]
self.publish(next_wps)
def log_excepthook(etype, evalue, tb):
_old_excepthook(etype, evalue, tb)
error(''.join(traceback.format_exception(etype, evalue, tb)))
def on_predict_3d(self, goal):
rospy.loginfo('nbv_3d_cnn_predict: action start.')
output_size_mult = 1
if (self.model_type == ''):
model_file_prefix = ''
output_channels = 1
output_size_mult = 2**self.sub_image_expand_pow
elif (self.model_type == 'flat'):
model_file_prefix = 'flat'
output_channels = 52
elif (self.model_type == 'circular'):
model_file_prefix = 'circular'
output_channels = 1
elif (self.model_type == 'quat'):
model_file_prefix = 'quat'
output_channels = 4
elif (self.model_type == 'autocomplete'):
model_file_prefix = 'autocomplete'
output_channels = 1
else:
rospy.logfatal('Unknown model type: ' + self.model_type)
exit(1)
image_width = goal.empty.layout.dim[2].size
image_height = goal.empty.layout.dim[1].size
image_depth = goal.empty.layout.dim[0].size
frontier_image_width = goal.frontier.layout.dim[2].size
frontier_image_height = goal.frontier.layout.dim[1].size
frontier_image_depth = goal.frontier.layout.dim[0].size
if (frontier_image_width != image_width
or frontier_image_height != image_height
or frontier_image_depth != image_depth):
rospy.error(
'nbv_3d_cnn_predict: empty image has size %d %d %d, but frontier image has size %d %d %d, aborted.'
%
(image_width, image_height, image_depth, frontier_image_width,
frontier_image_height, frontier_image_depth))
self.action_server.set_aborted()
return
np_empty_image = np.reshape(
np.array(goal.empty.data).astype('float32'),
[image_depth, image_height, image_width])
np_frontier_image = np.reshape(
np.array(goal.frontier.data).astype('float32'),
[image_depth, image_height, image_width])
input_image = [np_empty_image, np_frontier_image]
input_image = np.transpose(input_image, [1, 2, 3, 0])
input_shape = [image_depth, image_height, image_width]
load_input_shape = [image_depth, image_height, image_width, 2]
if (self.model == None or self.last_input_shape != input_shape):
rospy.loginfo(
'nbv_3d_cnn_predict: last input shape does not match, reloading model (type "%s").'
% self.model_type)
if (self.model_type == ''):
model = nbv_3d_model.get_3d_model(
self.sensor_range_voxels, load_input_shape,
self.sub_image_expand_pow, self.log_accuracy_skip_voxels)
elif (self.model_type == 'flat'):
model = nbv_3d_model.get_flat_3d_model(
self.sensor_range_voxels, load_input_shape, 52,
self.log_accuracy_skip_voxels)
elif (self.model_type == 'quat'):
model = nbv_3d_model.get_quat_3d_model(
self.sensor_range_voxels, load_input_shape,
self.log_accuracy_skip_voxels)
elif (self.model_type == 'autocomplete'):
model = nbv_3d_model.get_autocomplete_3d_model(
self.sensor_range_voxels, load_input_shape)
model.summary()
model.load_weights(self.checkpoint_file)
self.model = model
self.last_input_shape = input_shape
else:
model = self.model
rospy.loginfo('nbv_3d_cnn_predict: predicting.')
prediction = model.predict(np.array([
input_image,
]))
rospy.loginfo('nbv_3d_cnn_predict: sending result.')
prediction = prediction[0]
if (output_channels == 1):
prediction = np.transpose(prediction, [3, 0, 1, 2])
prediction = prediction[0]
pass
out_image_depth = len(prediction)
out_image_height = len(prediction[0])
out_image_width = len(prediction[0][0])
prediction = np.reshape(prediction.astype('float32'), [
out_image_width * out_image_height * out_image_depth *
output_channels,
])
if (len(prediction) > 10000):
rospy.loginfo(
'nbv_3d_cnn_predict: response is big, using raw publisher.')
self.raw_pub.publish(prediction)
result = nbv_3d_cnn_msgs.Predict3dResult()
if (len(prediction) <= 10000):
result.scores.data = prediction.tolist()
dim_x = std_msgs.MultiArrayDimension()
dim_x.label = "x"
dim_x.size = out_image_width
dim_x.stride = output_channels
dim_y = std_msgs.MultiArrayDimension()
dim_y.label = "y"
dim_y.size = out_image_height
dim_y.stride = out_image_width * output_channels
dim_z = std_msgs.MultiArrayDimension()
dim_z.label = "z"
dim_z.size = out_image_depth
dim_z.stride = out_image_width * out_image_height * output_channels
dim_c = std_msgs.MultiArrayDimension()
dim_c.label = "channels"
dim_c.size = output_channels
dim_c.stride = 1
layout_dims = [dim_z, dim_y, dim_x]
if (output_channels != 1):
layout_dims.append(dim_c)
result.scores.layout.dim = layout_dims
self.action_server.set_succeeded(result)
rospy.loginfo('nbv_3d_cnn_predict: action succeeded.')
pass
def on_predict(self, goal):
rospy.loginfo('nbv_3d_cnn_predict: action start.')
if (self.model_type == ''):
model_file_prefix = ''
output_channels = 1
elif (self.model_type == 'flat'):
model_file_prefix = 'flat'
output_channels = 1
elif (self.model_type == 'circular'):
model_file_prefix = 'circular'
output_channels = 1
elif (self.model_type == 'quat'):
model_file_prefix = 'scoreangle'
output_channels = 2
elif (self.model_type == 'autocomplete'):
model_file_prefix = 'autocomplete'
output_channels = 1
else:
rospy.logfatal('Unknown model type: ' + self.model_type)
exit(1)
image_width = goal.empty.width
image_height = goal.empty.height
if (goal.frontier.width != image_width
or goal.frontier.height != image_height):
rospy.error(
'nbv_3d_cnn_predict: empty image has size %d %d, but frontier image has size %d %d, aborted.'
% (image_width, image_height, goal.frontier.width,
goal.frontier.height))
self.action_server.set_aborted()
return
bridge = cv_bridge.CvBridge()
empty_image = bridge.imgmsg_to_cv2(goal.empty,
desired_encoding='mono8')
frontier_image = bridge.imgmsg_to_cv2(goal.frontier,
desired_encoding='mono8')
np_empty_image = empty_image
np_frontier_image = frontier_image
input_image = [np_empty_image, np_frontier_image]
input_image = np.transpose(input_image, [1, 2, 0])
input_image = input_image.astype(
'float32') / 255.0 # convert to zeros and ones
input_shape = [image_height, image_width]
if (self.model == None or self.last_input_shape != input_shape):
load_input_shape = [image_height, image_width, 2]
rospy.loginfo(
'nbv_3d_cnn_predict: last input shape does not match, reloading model (type "%s").'
% self.model_type)
#model = nbv_2d_model.get_2d_model(self.sensor_range_voxels, load_input_shape, 2)
if (self.model_type == ''):
model = nbv_2d_model.get_2d_model(self.sensor_range_voxels,
load_input_shape,
self.sub_image_expand_pow)
elif (self.model_type == 'flat'):
model = nbv_2d_model.get_flat_2d_model(
self.sensor_range_voxels, load_input_shape,
self.sub_image_expand_pow)
elif (self.model_type == 'quat'):
model = nbv_2d_model.get_quat_2d_model(
self.sensor_range_voxels, load_input_shape)
elif (self.model_type == 'autocomplete'):
model = nbv_2d_model.get_autocomplete_2d_model(
self.sensor_range_voxels, load_input_shape)
elif (self.model_type == 'circular'):
model = nbv_2d_model.get_circular_2d_model(
self.sensor_range_voxels, load_input_shape,
self.sub_image_expand_pow)
model.summary()
model.load_weights(self.checkpoint_file)
self.model = model
self.last_input_shape = input_shape
else:
model = self.model
rospy.loginfo('nbv_3d_cnn_predict: predicting.')
prediction = model.predict(np.array([
input_image,
]))
rospy.loginfo('nbv_3d_cnn_predict: sending result.')
prediction = prediction[0]
if (output_channels == 1):
prediction = np.transpose(prediction, [2, 0, 1])
prediction = prediction[0]
elif (output_channels == 2):
prediction = np.transpose(prediction, [2, 0, 1])
prediction = np.asarray([
prediction[0], prediction[1],
np.full((image_height * self.sub_image_expand,
image_width * self.sub_image_expand), 0.0)
])
prediction = np.transpose(prediction, [1, 2, 0])
pass
if (output_channels == 1):
encoding = "32FC1"
else:
encoding = "32FC3"
pass
prediction = prediction.astype('float32')
result = nbv_3d_cnn_msgs.PredictResult()
result.scores = bridge.cv2_to_imgmsg(prediction, encoding=encoding)
self.action_server.set_succeeded(result)
rospy.loginfo('nbv_3d_cnn_predict: action succeeded.')
pass
rospy.loginfo("\t diff %f", energy - past_energy)
rospy.loginfo("\t step %f", mu * (energy - past_energy))
rospy.loginfo("\t past theta %f", past_theta)
rospy.loginfo("\t current theta %f", theta)
pub.publish(theta)
past_energy = energy
past_theta = theta
rospy.loginfo("Starting energy2theta node...")
rospy.init_node('energy2theta', anonymous=True)
initial_angle_param_name = '/beamform/initial_angle'
if rospy.has_param(initial_angle_param_name):
past_theta = rospy.get_param(initial_angle_param_name)
rospy.loginfo("\t Initial angle: %f", past_theta)
else:
rospy.error(
"Parameter /beamform/initial_angle not found. Is beamform node running?"
)
exit()
try:
pub = rospy.Publisher('theta', Float32, queue_size=10)
rospy.Subscriber("jackaudio", JackAudio, energycallback)
except rospy.ROSInterruptException:
pass
rospy.spin()
def callback(self, msg):
service = rospy.ServiceProxy(self.service_name, TeleportAbsolute)
try:
service(msg.x + 5, msg.y + 5, msg.theta)
except rospy.ServiceException as exc:
rospy.error("Service did not process request: " + str(exc))
if foundFrame:
frameId = foundFrame.groups()[0]
blobDir = os.path.dirname(filename)
# Read the blobs that specify the fish
blobStream = open(filename)
try:
fishBlobs, imgName = blobSerializer.Deserialize(blobStream, blobDir)
finally:
blobStream.close()
# Find the bounds of the image
cvImg = cv2.imread(imgName)
if cvImg.shape[0] == 0 or cvImg.shape[1] == 0:
rospy.error("Could not open image: " + imgName)
continue
# Build up some blobs
random.seed(options.seed)
negBlobs = Blob.BlobResult()
while negBlobs.nBlobs() < options.nregions:
negBlobs.AppendBlob(GenerateNegativeBoxBlob(fishBlobs,
options.min_size,
(cvImg.shape[1],
cvImg.shape[0])))
# Write the blobs to a file
negBlobFile = open('%s%s.blob' % (prefix, frameId), 'w')
try:
blobSerializer.Serialize(negBlobFile,
def torque_cb(msg):
rospy.error("Torque control not implemented yet")
