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