Exemple #1
0
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")
Exemple #2
0
 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
Exemple #4
0
 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
Exemple #5
0
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()
Exemple #6
0
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
Exemple #8
0
    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'
Exemple #10
0
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)
Exemple #14
0
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
Exemple #17
0
            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()
Exemple #18
0
 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))
Exemple #19
0
    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,
Exemple #20
0
def torque_cb(msg):
    rospy.error("Torque control not implemented yet")