Exemple #1
0
 def __init__(self, accFrame, gyroFrame, outFrame):
     self.omega_out = np.array([0., 0., 0.])
     self.omega_cov_out = np.zeros([3,3])
     self.omega_dot = np.array([0., 0., 0.])
     self.a_out = np.array([0., 0., 0.])
     self.a_cov_out = np.zeros([3,3])
     self.rot_out = np.array([0., 0., 0.])
     
     self.tf_listener = tf.TransformListener()
     self.out_to_in_trans = np.array([0., 0., 0.])
     self.in_orientation = np.array([0.,0.,0.,1.])
     self.out_to_in_rot = np.zeros([3,3])            # turns to eye(3) or proper format after receiving first tf frame.
     self.out_to_in_rot_q = np.array([0.,0.,0.,1.])
     self.in_to_gyro_rot = np.zeros([3,3])           # turns to eye(3) or proper format after receiving first tf frame.
     self.in_to_gyro_rot_q = np.array([0.,0.,0.,1.])
     
     self.omega_last = np.array([0., 0., 0.])
     self.a_in = np.array([0., 0., 0.])
     self.a_cov_in = np.zeros([3,3])
     self.time_omega_old = Time()
     self.time_acc_old = Time()
     
     # in_frame should be the accelerometer frame. Gyro data shall be transformed to the accel frame accordingly.
     self.gyroFrame = gyroFrame
     self.inFrame = accFrame
     self.outFrame = outFrame
     self.pub_transformed = rospy.Publisher("imu_in_"+outFrame, Imu, queue_size=1)
Exemple #2
0
 def _parse_plan(self):
     # TODO this parser only accepts a set of goto commands
     """A plan has been requested by neptus to start. Parse the plan here and send to controller."""
     geopoints = []
     speeds = []
     for maneuver in self._current_plan.plan_spec.maneuvers:
         geopoints.append(
             GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
                      maneuver.maneuver.lon * 180.0 / np.pi,
                      maneuver.maneuver.z))
         speeds.append(maneuver.maneuver.speed)
     if self._datum is not None:
         geopoints.append(self._datum)
     req = ConvertGeoPointsRequest()
     req.geopoints = geopoints
     points = self._geo_converter.call(req).utmpoints
     wps = []
     # If datum is available, then reference frame is ENU World
     if self._datum is not None:
         for point, speed in zip(points[:-1], speeds):
             wp = Waypoint()
             wp.point = Point(point.x - points[-1].x,
                              point.y - points[-1].y,
                              -point.z - points[-1].z)
             wp.header.stamp = rospy.Time.now()
             wp.header.frame_id = "world"
             wp.radius_of_acceptance = 3.0
             wp.max_forward_speed = speed
             wp.use_fixed_heading = False
             wp.heading_offset = 0.0
             wps.append(wp)
     # If datum is not given, then reference frame is ENU UTM TODO: LOOKUP TRANSFORM
     else:
         for point, speed in zip(points, speeds):
             wp = Waypoint()
             wp.point.x = point.x
             wp.point.y = point.y
             wp.point.z = -point.z
             wp.header.stamp = rospy.Time.now()
             wp.header.frame_id = "utm"
             wp.radius_of_acceptance = 3.0
             wp.max_forward_speed = speed
             wp.use_fixed_heading = False
             wp.heading_offset = 0.0
             wps.append(wp)
     self.STATE = PlanControlState.READY
     req = InitWaypointSetRequest()
     req.start_time = Time(rospy.Time.from_sec(0))
     req.start_now = True
     req.waypoints = wps
     req.max_forward_speed = max(speeds)
     req.interpolator.data = "lipb"
     req.heading_offset = 0
     self._waypoint_setter.wait_for_service()
     if self._waypoint_setter(Time(rospy.Time.from_sec(0.0)), True, wps,
                              max(speeds), 0.0, String("lipb")):
         self.STATE = PlanControlState.EXECUTING
     else:
         self.STATE = PlanControlState.FAILURE
 def handleTimeRequest(self, data):
     """ Respond to device with system time. """
     t = Time()
     t.data = rospy.Time.now()
     data_buffer = StringIO.StringIO()
     t.serialize(data_buffer)
     self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
     self.lastsync = rospy.Time.now()
Exemple #4
0
 def handleTimeRequest(self, data):
     """ Respond to device with system time. """
     t = Time()
     t.data = rospy.Time.now()
     data_buffer = StringIO.StringIO()
     t.serialize(data_buffer)
     self.send(TopicInfo.ID_TIME, data_buffer.getvalue())
     self.lastsync = rospy.Time.now()
    def runSim(self, maxTime, incrTime, superRealTimeFactor):
        rospy.loginfo('=== RUN SIM (%.4f, %.4f, %.4f) ===' %
                      (maxTime, incrTime, superRealTimeFactor))

        rospy.loginfo('Unpausing simulation to unlock /clock')
        self.unpause_sim_cln(EmptyRequest())

        rospy.loginfo('Resetting robot pose')
        #self.reset_sim_cln(EmptyRequest())
        self.reset_sim_world_cln(EmptyRequest())

        rospy.loginfo('Resetting policy at start of runSim')
        self.reset_policy_cln(ResetPolicyRequest(self.theta))

        rospy.loginfo('Pausing simulation')
        self.pause_sim_cln(EmptyRequest())

        sim_start_time = rospy.Time.now()
        time_msg = Time()
        time_msg.data = sim_start_time
        rospy.loginfo('Announcing new episode start time')
        self.new_episode_pub.publish(time_msg)

        latest_real_time = time.time()
        for i in xrange(int(math.ceil(float(maxTime) / incrTime))):
            t = float(i * incrTime)
            desired_time = sim_start_time + rospy.Duration(t)

            step_policy_req = StepPolicyRequest(desired_time)
            rospy.loginfo('StepPolicy(%.4f)' %
                          (step_policy_req.desired_time).to_sec())
            self.step_policy_cln(step_policy_req)

            step_sim_req = RunSimulationUntilTimeRequest(desired_time)
            rospy.loginfo('StepSimulator(%.4f)' %
                          (step_sim_req.desired_time).to_sec())
            self.step_simulator_cln(step_sim_req)

            # For now we will sleep for some time before stepping next; in reality we would go as fast as plant lets us
            sleep_dur = incrTime / superRealTimeFactor
            now = time.time()
            real_time_diff = now - latest_real_time
            if real_time_diff < sleep_dur:
                try:
                    time.sleep(sleep_dur - real_time_diff)
                except KeyboardInterrupt:
                    break
            latest_real_time = time.time()

        rospy.loginfo('Resetting policy at end of runSim')
        self.reset_policy_cln(ResetPolicyRequest(self.theta))

        rospy.loginfo('Unpausing simulation to unlock /clock')
        self.unpause_sim_cln(EmptyRequest())

        rospy.loginfo('runSim done!')
Exemple #6
0
    def timer_callback(self, data):
        self.time -= self.unit

        if self.time < 0:
            self.time = 0

        msg = Time()

        msg.data = rospy.Time(secs=self.time)

        self.pub.publish(msg)
 def timeTimerCallback(self, event):
     with self.lock:
         if self.last_send_time:
             self.last_send_time_pub.publish(self.last_send_time)
         else:
             self.last_send_time_pub.publish(Time(data=rospy.Time(0)))
         if self.last_input_received_time:
             self.last_input_received_time_pub.publish(
                 self.last_input_received_time)
         else:
             self.last_input_received_time_pub.publish(
                 Time(data=rospy.Time(0)))
 def update(self, pressed, timestamp):
     if not pressed and self.last_state:  # only when released
         rospy.logdebug(
             "MagicButtonRelay : stamped event for button '%s' published at '%s'"
             % (self.button_id, self.publisher.name))
         self.publisher.publish(Time(timestamp))
     self.last_state = pressed
def iso8601_to_rostime(iso):
    """Converts ISO 8601 time to ROS Time.

    Args:
        iso: ISO 8601 encoded string.

    Returns:
        std_msgs/Time.
    """
    # Convert to datetime in UTC.
    t = dateutil.parser.parse(iso)
    if not t.utcoffset():
        t = t.replace(tzinfo=tzutc())

    # Convert to time from epoch in UTC.
    epoch = datetime.utcfromtimestamp(0)
    epoch = epoch.replace(tzinfo=tzutc())
    dt = t - epoch

    # Create ROS message.
    time = Time()
    time.data.secs = int(dt.total_seconds())
    time.data.nsecs = dt.microseconds * 1000

    return time
Exemple #10
0
    def execute(self, userdata):
        target_waypoint_msg = WaypointMsg()
        target_waypoint_msg = userdata.target_waypoint.to_message()
        
        print 'waiting for go to waypoint server'
        rospy.wait_for_service('anahita/go_to')

        go_to = rospy.ServiceProxy('anahita/go_to', GoTo)

        start_time = Time()
        start_time.data.secs = rospy.get_rostime().to_sec()
        start_time.data.nsecs = rospy.get_rostime().to_nsec()
        
        interpolator = String()
        interpolator.data = 'cubic_interpolator'
        print 'adding waypoints....'
        resp = init_waypoint_set(max_forward_speed = 0.5, 
                                interpolator = interpolator,
                                waypoint = target_waypoint_msg)

        then = rospy.get_time()

        while not rospy.is_shutdown():
            now = rospy.get_time()
            dt = now - then
            dist = calc_dist(self._pose, target_waypoint_msg)
            
            if dist < self._threshold:
                return 'success'

            if dt > self._timeout:
                return 'timeout'

            if dist > self._cutoff:
                return 'failed'
 def test_dictionary_with_time(self):
     from std_msgs.msg import Time
     now_time = rospy.Time.now()
     expected_message = Time(data=now_time)
     dictionary = {'data': {'secs': now_time.secs, 'nsecs': now_time.nsecs}}
     message = message_converter.convert_dictionary_to_ros_message(
         'std_msgs/Time', dictionary)
     self.assertEqual(message, expected_message)
    def transform_callback(self, data, msg_type):
        """ ROS callback.
        """
        a = time.time()

        self.num_messages_received += 1
        self.lastbeat = time.time()
        # Get frame IDs of the objects to which the ROS messages are referred.
        node_id0 = data.header.frame_id
        if msg_type == "AprilTagDetection":
            node_id1 = str(data.tag_id)
            pose_error = float(data.pose_error) * 10**8 / 37.0
            # self.pose_errors.append(pose_error)
            # var = np.var(self.pose_errors)
            # mean = np.mean(self.pose_errors)
            # print("pose error : mean %f, var %f" % (mean, var))
        elif msg_type == "TransformStamped":
            node_id1 = data.child_frame_id
        else:
            raise Exception(
                "Transform callback received unsupported msg_type %s" %
                msg_type)

        # Convert the frame IDs to the right format.
        node_id0 = self.filter_name(node_id0)
        node_id1 = self.filter_name(node_id1)

        # Ignore messages from one watchtower to another watchtower (i.e.,
        # odometry messages between watchtowers).
        is_from_watchtower = False
        if (node_id0.startswith("watchtower")):
            is_from_watchtower = True
            if (node_id1.startswith("watchtower")):
                # print(data)
                return 0

        # Create translation vector.
        header_time = Time(data.header.stamp)
        time_stamp = header_time.data.secs + header_time.data.nsecs * 10**(-9)

        if self.first_time_stamp == -1:
            self.first_time_stamp = time_stamp
            self.resampler.signal_reference_time_stamp(self.first_time_stamp)

        if (node_id1 == node_id0):
            # Same ID: odometry message, e.g. the same Duckiebot sending
            # odometry information at different instances in time.
            self.handle_odometry_message(node_id1, data, time_stamp)
        elif (is_from_watchtower):
            # Tag detected by a watchtower.
            self.handle_watchtower_message(node_id0, node_id1, data,
                                           time_stamp, pose_error)
        else:
            # Tag detected by a Duckiebot.
            self.handle_duckiebot_message(node_id0, node_id1, data, time_stamp,
                                          pose_error)
        b = time.time()
        self.callback_times.append(b - a)
Exemple #13
0
    def dji_gps_position_cb(self, gps_msg):
        if self.gps_record == True:
            rostime = Time()
            rostime.data = rospy.get_rostime()
            self.gpsA3_bag.write('rostime', rostime)
            self.gpsA3_bag.write('geoPosition', gps_msg)

        if self.telemetry_save:
            if not self.telemetry_file_head:
                self.telemetry_file_writer.writerow({'Time': 'Time', 'Latitude':'Latitude', 'Longitude':'Longitude', 'Altitude':'Altitude', \
                                                'Laser_altitude':'Laser_altitude', 'Quat.x':'Quat.x', \
                                                'Quat.y':'Quat.y', 'Quat.z':'Quat.z', 'Quat.w':'Quat.w'})
                self.telemetry_file_head = True

            else:
                self.telemetry_file_writer.writerow({'Time':rospy.get_rostime(), 'Latitude':gps_msg.latitude, 'Longitude':gps_msg.longitude, 'Altitude':gps_msg.altitude, \
                                                'Laser_altitude':self.current_laser_altitude, 'Quat.x':self.current_attitude.quaternion.x, \
                                                'Quat.y':self.current_attitude.quaternion.y, 'Quat.z':self.current_attitude.quaternion.z, 'Quat.w':self.current_attitude.quaternion.w})
Exemple #14
0
 def test_ros_message_with_time(self):
     from std_msgs.msg import Time
     rospy.init_node('time_node')
     now_time = rospy.Time.now()
     expected_dictionary = {
         'data': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }
     }
     message = Time(data=now_time)
     dictionary = message_converter.convert_ros_message_to_dictionary(message)
     self.assertEqual(dictionary, expected_dictionary)
Exemple #15
0
    def transform_callback(self, data):
        """ ROS callback.
        """
        self.num_messages_received += 1
        # Get frame IDs of the objects to which the ROS messages are referred.
        id0 = data.header.frame_id
        id1 = data.child_frame_id
        # Convert the frame IDs to the right format.
        id0 = self.filter_name(id0)
        id1 = self.filter_name(id1)

        # Ignore messages from one watchtower to another watchtower (i.e.,
        # odometry messages between watchtowers). TODO: check if we can avoid
        # sending these messages.
        is_from_watchtower = False
        if (id0.startswith("watchtower")):
            is_from_watchtower = True
            if (id1.startswith("watchtower")):
                # print(data)
                return 0

        # Create translation vector.
        t = [
            data.transform.translation.x, data.transform.translation.y,
            data.transform.translation.z
        ]

        # Create rotation matrix. NOTE: in Pygeometry, quaternion is
        # the (w, x, y, z) form.
        q = [
            data.transform.rotation.w, data.transform.rotation.x,
            data.transform.rotation.y, data.transform.rotation.z
        ]
        M = g.rotations.rotation_from_quaternion(np.array(q))

        # Verify that the rotation is a proper rotation.
        det = np.linalg.det(M)
        if (det < 0):
            print("det is %f" % det)

        # Obtain complete transform and use it to add vertices in the graph.
        transform = g2o.Isometry3d(M, t)
        time = Time(data.header.stamp)
        time_stamp = time.data.secs + time.data.nsecs * 10**(-9)

        if (id1 == id0):
            # Same ID: odometry message, e.g. the same Duckiebot sending
            # odometry information at different instances in time.
            self.handle_odometry_message(id1, transform, time_stamp)
        elif (is_from_watchtower):
            # Tag detected by a watchtower.
            self.handle_watchtower_message(id0, id1, transform, time_stamp)
        else:
            # Tag detected by a Duckiebot.
            self.handle_duckiebot_message(id0, id1, transform, time_stamp)
def parse_request(mis, utm_frame_id="utm", radius_of_acceptance=5.0):
    with open(mis, "r") as f:
        while not f.readline().startswith("START"):
            pass
        geopoints = []
        headings = []
        speeds = []
        line = f.readline().rstrip()
        while not line.startswith("END"):
            values = line.split(";")
            values = [value.strip() for value in values]
            params = parse_iver_format(values[5])
            zvar = -float(params[0][1:]) * 0.3048 if params[0][
                0] == 'D' else float(params[0][1:]) * 0.3048
            geopoints.append(GeoPoint(float(values[1]), float(values[2]),
                                      zvar))
            headings.append(float(values[4]))
            speeds.append(float(params[-1][1:]) * 0.5144)
            line = f.readline().rstrip()
        # while not f.readline().startswith("START VEHICLE"):
        #     pass
        # line = f.readline().rstrip()
        # while not line.startswith("END VEHICLE"):
        #     if line.startswith("UVC=WPRadius"):
        #         roas.append(float(line.split("=")[-1]))
        #         break

    req = ConvertGeoPointsRequest()
    req.geopoints = geopoints
    p = rospy.ServiceProxy("convert_points", ConvertGeoPoints)
    p.wait_for_service()
    res = p(req)
    wps = []
    for point, heading, speed in zip(res.utmpoints, headings, speeds):
        wp = Waypoint()
        wp.point.x = point.x
        wp.point.y = point.y
        wp.point.z = point.z
        wp.header.stamp = rospy.Time.now()
        wp.header.frame_id = utm_frame_id
        wp.radius_of_acceptance = radius_of_acceptance
        wp.max_forward_speed = speed
        wp.use_fixed_heading = False
        wp.heading_offset = 0.0
        wps.append(wp)
    req = InitWaypointSetRequest()
    req.start_time = Time(rospy.Time.from_sec(0))
    req.start_now = True
    req.waypoints = wps
    req.max_forward_speed = max(speeds)
    req.interpolator = String("lipb")
    req.heading_offset = 0
    req.max_forward_speed = max(speeds)
    req.waypoints = wps
    return req
 def test_ros_message_with_time(self):
     from std_msgs.msg import Time
     from time import time
     now_time = rospy.Time(time())
     expected_dictionary = {
         'data': { 'secs': now_time.secs, 'nsecs': now_time.nsecs }
     }
     message = Time(data=now_time)
     message = serialize_deserialize(message)
     dictionary = message_converter.convert_ros_message_to_dictionary(message)
     self.assertEqual(dictionary, expected_dictionary)
def callback_dis(msg):
	global pub, models, count, pixels, obj_R, obj_L

	count += 1
	if count == 1:
		pixels = np.append(pixels, np.array([msg.vector.x, msg.vector.y]))
		pixels = pixels.reshape(1, -1)
	else:
		pixels = np.append(pixels, np.array([msg.vector.x, msg.vector.y]).reshape(1, -1), axis=0)

	if pixels.size == 8: # {3 pixels : 6, 4 pixels : 8, 5 pixels: 10, 6 pixels : 12}
		clf = pickle.load(open(models[count], 'rb'))		
		prediction = clf.predict(np.array([[distance.euclidean([msg.vector.x, msg.vector.y], obj_R), distance.euclidean([msg.vector.x, msg.vector.y], obj_L)]]))
		pred_msg = Bool()
		pred_msg.data = True if prediction == 1 else False
		rospy.loginfo('Predicted {} '.format(prediction))
		pub.publish(pred_msg)
		prediction_time = Time()
		prediction_time.data = rospy.Time.now()
		prediction_time_pub.publish(prediction_time)
    def handleTimeRequest(self, data):
        """ Respond to device with system time. """

        t = Time()
        t.deserialize(data)

        if t.data.secs == 0 and t.data.nsecs == 0:
            t = Time()
            t.data = rospy.Time.now()
            data_buffer = StringIO.StringIO()
            t.serialize(data_buffer)
            self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
            self.lastsync = rospy.Time.now()
        else:
            # TODO sync the time somehow
            self.lastsync = rospy.Time.now()
            pass
Exemple #20
0
def talker(filename, topicName='PoseStamped'):
    pub = rospy.Publisher('iiwa/poseFromFile/' + topicName,
                          PoseStamped,
                          queue_size=1)
    time_pub = rospy.Publisher('iiwa/poseFromFile/receiveTime',
                               Time,
                               queue_size=1)
    #rosTime_pub = rospy.Publisher('poseFromFile/receiveRosTime', Time, queue_size=1)
    rospy.init_node('posePublisher', anonymous=True)
    # Without delay the first few values could not be received by the subscriber despite it had been
    # already registered. Apparently it takes some time until the connection is established after the
    # publisher (node) is created.
    time.sleep(0.5)
    rate = rospy.Rate(SAMPLE_RATE)
    reader = csv.reader(open(filename))

    for line in reader:
        if rospy.is_shutdown(): break
        values = [float(x) for x in line]
        #rospy.loginfo(values)
        receiveTime = Time()
        #receiveRosTime = Time()
        receiveTime.data = rospy.Time.from_sec(time.time())
        #receiveRosTime.data = rospy.Time.now()
        pose = PoseStamped()
        pose.header.frame_id = "/operator"
        pose.header.stamp = rospy.Time.now()
        pose.pose.position.x = values[0]
        pose.pose.position.y = values[1]
        pose.pose.position.z = values[2]
        pose.pose.orientation.x = values[3]
        pose.pose.orientation.y = values[4]
        pose.pose.orientation.z = values[5]
        pose.pose.orientation.w = values[6]
        pub.publish(pose)
        time_pub.publish(receiveTime)
        #rosTime_pub.publish(receiveRosTime)
        rate.sleep()
Exemple #21
0
    def rgb_camera_capture_function(self):
        try:
            time_now = datetime.datetime.now()
            print('Capturing image')
            file_path = gp.check_result(
                gp.gp_camera_capture(self.rgb_camera, gp.GP_CAPTURE_IMAGE))
            filename = 'image_{0}.jpg'.format(
                time_now.strftime("%Y-%m-%d %H:%M:%S"))
            target = os.path.join(self.rgb_images_dir, filename)
            print('Copying image to', target)
            camera_file = gp.check_result(
                gp.gp_camera_file_get(self.rgb_camera, file_path.folder,
                                      file_path.name, gp.GP_FILE_TYPE_NORMAL))
            gp.check_result(gp.gp_file_save(camera_file, target))

            rostime = Time()
            rostime.data = rospy.get_rostime()
            self.rgb_file_writer.writerow({
                'RGB Image': filename,
                'Time': rospy.get_rostime()
            })
        except:
            rospy.logerr("PAL: Error in connection with RGB camera")
    def runLive(self, maxTime):
        time.sleep(
            0.5
        )  # not sure why need this delay, but otherwise /aqua_rl/new_episode doesn't publish properly

        rospy.loginfo('=== RUN LIVE (%.4f) ===' % maxTime)

        rospy.loginfo('Resetting robot pose')
        #self.reset_sim_cln(EmptyRequest())
        self.reset_sim_world_cln(EmptyRequest())

        rospy.loginfo('Resetting policy at start of runLive')
        self.reset_policy_cln(ResetPolicyRequest(self.theta))

        time_msg = Time()
        time_msg.data = rospy.Time.now()
        rospy.loginfo('Announcing new episode start time')
        self.new_episode_pub.publish(time_msg)

        rospy.loginfo('Unpausing simulation')
        self.unpause_sim_cln(EmptyRequest())

        rospy.loginfo('Publishing run_live')
        self.run_live_pub.publish(EmptyMsg())

        rospy.loginfo(
            'Sleeping for %.2f seconds before stopping live episode' % maxTime)
        try:
            time.sleep(maxTime)
        except KeyboardInterrupt:
            pass

        rospy.loginfo('Resetting policy at end of runLive')
        self.reset_policy_cln(ResetPolicyRequest(self.theta))

        rospy.loginfo('runLive done!')
Exemple #23
0
def handleTimeCal(req):
    global local_time
    # there may be a case that user send time request even before "local_time" topic receive data 
    if local_time != None:
        remote = Time()
        # list of choise
        if req.location == "Japan":
            # time differece here is 11 hour
            remote = local_time - rospy.Duration(hourToSec(11))
        elif req.location == "Australia":
            # time differece here is 10 hour
            remote = local_time - rospy.Duration(hourToSec(10))
        
        rospy.loginfo("There is a request coming from Kent at " + str(datetime.fromtimestamp(local_time.to_sec())))
        #return result back to client
        return timeserverResponse(remote)
 def _send_goal(self, req):
     res = InitWaypointSetResponse()
     res.success = False
     try:
         self._mode_client.wait_for_service(rospy.Duration(5))
         mode_req = SetControlModeRequest()
         mode_req.mode.mode = mode_req.mode.LOSGUIDANCE
         self._mode_client(mode_req)
         self._action_client.wait_for_server(rospy.Duration(5))
         goal = FollowWaypointsGoal()
         goal.waypoints.header = Header(0, rospy.Time.now(), "utm")
         goal.waypoints.start_time = Time(goal.waypoints.header.stamp)
         goal.waypoints.waypoints = req.waypoints
         self._marker_pub.publish(goal.waypoints)
         self._action_client.send_goal(goal, self._handle_done,
                                       self._handle_active,
                                       self._handle_feedback)
         res.success = True
         return res
     except Exception as e:
         rospy.logerr("{} | {}".format(rospy.get_name(), e.message))
         return res
    def __init__(self):
        self._pos_token_glob_x = []
        self._pos_token_glob_y = []
        self.blob_sub = rospy.Subscriber('block_data', PixyData,
                                         self.blobb_callback)
        self._blob_y = 0.0
        self._blob_x = 0.0

        self._last_stamp = Time()
        # self._pose_pub_sub = rospy.Subscriber('pose', PoseStamped, self.pose_pub_callback)
        self._pose_pub_sub = rospy.Subscriber('robot_pose', Pose,
                                              self.pose_pub_callback)
        self._current_pose_pub = None
        self._current_x_pub = None
        self._current_y_pub = None
        self._current_orientation_pub = None
        self._phi_pub = None
        print 'wait pose'
        # rospy.wait_for_message('pose', PoseStamped)
        rospy.wait_for_message('robot_pose', Pose)
        print 'got pose'
        self._found_x = []
        self._found_y = []
        self._interrupt_pub = rospy.Subscriber('check_token', CorrectPosSrv,
                                               self.interrupt_callback)
        msg = rospy.wait_for_message("map", OccupancyGrid)
        meta_data = msg.info
        self._offset_x = meta_data.origin.position.x
        self._offset_y = meta_data.origin.position.y
        self._resolution = meta_data.resolution
        self._tl = tf.TransformListener()

        self._turtlebot_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
        self._move_base_client = actionlib.SimpleActionClient(
            'move_base', MoveBaseAction)
        self._move_base_client.wait_for_server()
        self._current_goal_x = 0.0
        self._current_goal_y = 0.0
        print 'init finished'
 def _parse_plan(self):
     # TODO this parser only accepts a set of goto commands
     """A plan has been requested by neptus to start. Parse the plan here and send to controller."""
     geopoints = []
     speeds = []
     is_depths = []
     for maneuver in self._current_plan.plan_spec.maneuvers:
         geopoints.append(
             GeoPoint(maneuver.maneuver.lat * 180.0 / np.pi,
                      maneuver.maneuver.lon * 180.0 / np.pi,
                      maneuver.maneuver.z))
         speeds.append(maneuver.maneuver.speed)
         is_depths.append(maneuver.maneuver.z_units == 1)
     req = ConvertGeoPointsRequest()
     req.geopoints = geopoints
     points = self._geo_converter.call(req).utmpoints
     wps = []
     # If datum is available, then reference frame is ENU World
     for point, speed, is_depth in zip(points, speeds, is_depths):
         wp = Waypoint()
         wp.point.x = point.x
         wp.point.y = point.y
         wp.point.z = -abs(point.z) if is_depth else abs(point.z)
         wp.header.stamp = rospy.Time.now()
         wp.header.frame_id = "utm"
         wp.radius_of_acceptance = 1.0
         wp.max_forward_speed = speed
         wp.use_fixed_heading = False
         wp.heading_offset = 0.0
         wps.append(wp)
     req = InitWaypointSetRequest()
     req.start_time = Time(rospy.Time.from_sec(0))
     req.start_now = True
     req.waypoints = wps
     req.max_forward_speed = max(speeds)
     req.interpolator.data = "lipb"
     req.heading_offset = 0
     self._send_goal(req)
Exemple #27
0
    def odom_cb(self, odom_msg):
        if self.start_time == 0:
            self.start_time = rospy.Time.now()

        self.odom_msg = odom_msg

    def ref_cb(self, odom_msg):
        self.ref_msg = odom_msg


if __name__ == '__main__':
    rospy.init_node('bag_data_node')
    node = BagDataNode()

    time_elapsed = Time()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        curr_time = rospy.Time.now()

        if curr_time.to_sec() - node.start_time.to_sec(
        ) > 1e-4 and not node.use_model and curr_time.to_sec(
        ) - node.start_time.to_sec() >= 30:  # 30 seconds
            node.dyn_reconfig_client.update_configuration({"use_model": True})
            node.use_model = True
            print("------- Using model -------")

        if curr_time.to_sec() - node.start_time.to_sec(
        ) > 1e-4 and curr_time.to_sec() - node.start_time.to_sec() >= 120:
            node.bag.close()
    if params['n_points'] <= 2:
        raise rospy.ROSException('Number of points must be at least 2')

    if params['max_forward_speed'] <= 0:
        raise rospy.ROSException('Velocity limit must be positive')

    try:
        rospy.wait_for_service('start_helical_trajectory', timeout=2)
    except rospy.ROSException:
        raise rospy.ROSException('Service not available! Closing node...')

    try:
        traj_gen = rospy.ServiceProxy('start_helical_trajectory',
                                      InitHelicalTrajectory)
    except rospy.ServiceException, e:
        raise rospy.ROSException('Service call failed, error=' + e)

    print 'Generating trajectory that starts at t=%fs' % start_time

    success = traj_gen(
        Time(rospy.Time(start_time)), start_now, params['radius'],
        Point(params['center'][0], params['center'][1],
              params['center'][2]), False, 0.0, params['n_points'],
        params['heading_offset'] * pi / 180, params['max_forward_speed'],
        params['duration'], params['n_turns'], params['delta_z'])

    if success:
        print 'Trajectory successfully generated!'
    else:
        print 'Failed'
    def __init__(self):

        # ad-hoc solution for separate publishing of wheel data
        self.xyyaw = Point()
        self.odom_msg = Odometry()
        self.fl_str = 0
        self.fr_str = 0
        self.rl_str = 0
        self.rr_str = 0
        self.avg_vel = 0
        self.curv = 0
        self.msg_state = [0, 0, 0, 0, 0, 0]

        self.point_x = list()
        self.point_y = list()
        self.est_yaw = 0
        self.yaw_first = 0
        self.count = 30

        # odometry info ---------------------
        self.last_t = time.time()
        self.last_t2 = time.time()
        self.x_global = 0
        self.y_global = 0
        self.last_omega = 0
        self.last_v = 0
        self.yaw_global = 0
        self.yaw_old = 0
        self.angle_buffer = []
        self.euler = []

        self.time_stamp = Time()

        self.init_flag = True

        # self.sub_fl_str_ = rospy.Subscriber("/communication/dspace/flwheel_steering", Float32, self.cb1, queue_size=1)
        # self.sub_fr_str_ = rospy.Subscriber("/communication/dspace/frwheel_steering", Float32, self.cb2, queue_size=1)
        # self.sub_rl_str_ = rospy.Subscriber("/communication/dspace/rlwheel_steering", Float32, self.cb3, queue_size=1)
        # self.sub_rr_str_ = rospy.Subscriber("/communication/dspace/rrwheel_steering", Float32, self.cb4, queue_size=1)
        # self.sub_avg_vel_ = rospy.Subscriber("/communication/dspace/avg_speed", Float32, self.cb5, queue_size=1)
        # self.sub_curv = rospy.Subscriber("/communication/dspace/curvature", Float32, self.cb6, queue_size=1)
        self.sub_yaw = rospy.Subscriber("/communication/dspace/imu",
                                        Imu,
                                        self.cb6,
                                        queue_size=1)
        self.sub_est = rospy.Subscriber("/odometry/filtered",
                                        Odometry,
                                        self.call_back_est,
                                        queue_size=1)

        self.pub_xyyaw_test = rospy.Publisher("/xyyaw_test",
                                              Point,
                                              queue_size=1)
        self.pub_odom_test = rospy.Publisher("/odom_test",
                                             Odometry,
                                             queue_size=1)
        # -----------------------------------------------------

        self.sub_wheel_data_all_ = rospy.Subscriber(
            "/communication/dspace/wheel_data_all",
            Float32MultiArray,
            self.callback,
            queue_size=1)
        self.pub_wheel_odometry_ = rospy.Publisher('Wheel_Odometry',
                                                   Odometry,
                                                   queue_size=1)

        self.wheel_data = list()

        # for some visualization --------------------
        self.sample_count = 10

        # for parameter tuning
        self.pose_cov = rospy.get_param("/wheel_pose_cov")
        self.twist_cov = rospy.get_param("/wheel_twist_cov")
    def run(self):
        """ Forward recieved messages to appropriate publisher. """
        data = ''
        while not rospy.is_shutdown():
            if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3):
                rospy.logerr("Lost sync with device, restarting...")
                self.requestTopics()
                self.lastsync = rospy.Time.now()

            flag = [0,0]
            flag[0]  = self.port.read(1)
            if (flag[0] != '\xff'):
                continue
            flag[1] = self.port.read(1)
            if ( flag[1] != '\xff'):
                rospy.loginfo("Failed Packet Flags ")
                continue
            # topic id (2 bytes)
            header = self.port.read(4)
            if (len(header) != 4):
                #self.port.flushInput()
                continue

            topic_id, msg_length = struct.unpack("<hh", header)
            msg = self.port.read(msg_length)
            if (len(msg) != msg_length):
                rospy.loginfo("Packet Failed :  Failed to read msg data")
                #self.port.flushInput()
                continue
            chk = self.port.read(1)
            checksum = sum(map(ord,header) ) + sum(map(ord, msg)) + ord(chk)

            if checksum%256 == 255:
                if topic_id == TopicInfo.ID_PUBLISHER:
                    try:
                        m = TopicInfo()
                        m.deserialize(msg)
                        self.senders[m.topic_id] = Publisher(m.topic_name, m.message_type)
                        rospy.loginfo("Setup Publisher on %s [%s]" % (m.topic_name, m.message_type) )
                    except Exception as e:
                        rospy.logerr("Failed to parse publisher: %s", e)
                elif topic_id == TopicInfo.ID_SUBSCRIBER:
                    try:
                        m = TopicInfo()
                        m.deserialize(msg)
                        self.receivers[m.topic_name] = [m.topic_id, Subscriber(m.topic_name, m.message_type, self)]
                        rospy.loginfo("Setup Subscriber on %s [%s]" % (m.topic_name, m.message_type))
                    except Exception as e:
                        rospy.logerr("Failed to parse subscriber. %s"%e)
                elif topic_id == TopicInfo.ID_SERVICE_SERVER:
                    try:
                        m = TopicInfo()
                        m.deserialize(msg)
                        self.senders[m.topic_id]=ServiceServer(m.topic_name, m.message_type, self)
                        rospy.loginfo("Setup ServiceServer on %s [%s]"%(m.topic_name, m.message_type) )
                    except:
                        rospy.logerr("Failed to parse service server")
                elif topic_id == TopicInfo.ID_SERVICE_CLIENT:
                    pass

                elif topic_id == TopicInfo.ID_PARAMETER_REQUEST:
                    self.handleParameterRequest(msg)

                elif topic_id == TopicInfo.ID_LOG:
                    self.handleLogging(msg)

                elif topic_id == TopicInfo.ID_TIME:
                    t = Time()
                    t.data = rospy.Time.now()
                    data_buffer = StringIO.StringIO()
                    t.serialize(data_buffer)
                    self.send( TopicInfo.ID_TIME, data_buffer.getvalue() )
                    self.lastsync = rospy.Time.now()
                elif topic_id >= 100: # TOPIC
                    try:
                        self.senders[topic_id].handlePacket(msg)
                    except KeyError:
                        rospy.logerr("Tried to publish before configured, topic id %d" % topic_id)
                else:
                    rospy.logerr("Unrecognized command topic!")
                rospy.sleep(0.001)
Exemple #31
0
            pos = numpy.matrix(s['Pos']).transpose()
            dir = numpy.matrix(s['Dir']).transpose()
            feat_name = '%s/%s.%d' % (obj.name, ITasCFeature.NAMES[featureT],
                                      counters[featureT])
            f = ITasCFeature(featureT, feat_name, pos, dir)
            obj.features.append(f)
            print '  ITasCFeature: %s, %s, pos:%s, dir:%s' % (
                feat_name, ITasCFeature.NAMES[featureT], str(pos), str(dir))
            counters[featureT] += 1

    # start ros node and broadcast all poses for each single frame
    pub = rospy.Publisher('handtracking', Feature)
    #rospy.init_node('handtracking')
    for frame_idx, frame in enumerate(frames):
        rot_mats = frame['mats']
        timestamp = Time(time.time())
        for i, (obj, rot_mat) in enumerate(zip(object_store, rot_mats)):
            if len(obj.features) == 0: continue
            #obj_pos = rot_mat[0:3,3]
            #obj_dir = rot_mat[0:3,0:3]
            for feature in obj.iterFeaturePoses(rot_mat):
                msg_frame = 'ref_frame_name'
                pos_vect = Vector3(float(feature.pos[0]),
                                   float(feature.pos[1]),
                                   float(feature.pos[2]))
                dir_vect = Vector3(float(feature.dir[0]),
                                   float(feature.dir[1]),
                                   float(feature.dir[2]))
                contactdir_vect = Vector3(0, 0, 0)
                feat_msg = Feature(msg_frame, feature.type, feature.name,
                                   pos_vect, dir_vect, contactdir_vect)
 def requestSyncTime(self):
     t = Time()
     data_buffer = StringIO.StringIO()
     t.serialize(data_buffer)
     self.send(TopicInfo.ID_TIME, data_buffer.getvalue())
Exemple #33
0
    def yuv_callback(self, yuv_frame):
        # Use OpenCV to convert the yuv frame to RGB
        info = yuv_frame.info(
        )  # the VideoFrame.info() dictionary contains some useful information such as the video resolution
        rospy.logdebug_throttle(10, "yuv_frame.info = " + str(info))
        cv2_cvt_color_flag = {
            olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
            olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
        }[info["yuv"]["format"]]  # convert pdraw YUV flag to OpenCV YUV flag
        cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)

        # Publish image
        msg_image = self.bridge.cv2_to_imgmsg(cv2frame, "bgr8")
        self.pub_image.publish(msg_image)

        # yuv_frame.vmeta() returns a dictionary that contains additional metadata from the drone (GPS coordinates, battery percentage, ...)
        metadata = yuv_frame.vmeta()
        rospy.logdebug_throttle(10, "yuv_frame.vmeta = " + str(metadata))

        if metadata[1] != None:
            header = Header()
            header.stamp = rospy.Time.now()
            header.frame_id = '/body'

            frame_timestamp = metadata[1][
                'frame_timestamp']  # timestamp [millisec]
            msg_time = Time()
            msg_time.data = frame_timestamp  # secs = int(frame_timestamp//1e6), nsecs = int(frame_timestamp%1e6*1e3)
            self.pub_time.publish(msg_time)

            drone_quat = metadata[1]['drone_quat']  # attitude
            msg_attitude = QuaternionStamped()
            msg_attitude.header = header
            msg_attitude.quaternion = Quaternion(drone_quat['x'],
                                                 -drone_quat['y'],
                                                 -drone_quat['z'],
                                                 drone_quat['w'])
            self.pub_attitude.publish(msg_attitude)

            location = metadata[1][
                'location']  # GPS location [500.0=not available] (decimal deg)
            msg_location = PointStamped()
            if location != {}:
                msg_location.header = header
                msg_location.header.frame_id = '/world'
                msg_location.point.x = location['latitude']
                msg_location.point.y = location['longitude']
                msg_location.point.z = location['altitude']
                self.pub_location.publish(msg_location)

            ground_distance = metadata[1]['ground_distance']  # barometer (m)
            self.pub_height.publish(ground_distance)

            speed = metadata[1]['speed']  # opticalflow speed (m/s)
            msg_speed = Vector3Stamped()
            msg_speed.header = header
            msg_speed.header.frame_id = '/world'
            msg_speed.vector.x = speed['north']
            msg_speed.vector.y = -speed['east']
            msg_speed.vector.z = -speed['down']
            self.pub_speed.publish(msg_speed)

            air_speed = metadata[1][
                'air_speed']  # air speed [-1=no data, > 0] (m/s)
            self.pub_air_speed.publish(air_speed)

            link_goodput = metadata[1][
                'link_goodput']  # throughput of the connection (b/s)
            self.pub_link_goodput.publish(link_goodput)

            link_quality = metadata[1]['link_quality']  # [0=bad, 5=good]
            self.pub_link_quality.publish(link_quality)

            wifi_rssi = metadata[1][
                'wifi_rssi']  # signal strength [-100=bad, 0=good] (dBm)
            self.pub_wifi_rssi.publish(wifi_rssi)

            battery_percentage = metadata[1][
                'battery_percentage']  # [0=empty, 100=full]
            self.pub_battery.publish(battery_percentage)

            state = metadata[1][
                'state']  # ['LANDED', 'MOTOR_RAMPING', 'TAKINGOFF', 'HOWERING', 'FLYING', 'LANDING', 'EMERGENCY']
            self.pub_state.publish(state)

            mode = metadata[1][
                'mode']  # ['MANUAL', 'RETURN_HOME', 'FLIGHT_PLAN', 'TRACKING', 'FOLLOW_ME', 'MOVE_TO']
            self.pub_mode.publish(mode)

            msg_pose = PoseStamped()
            msg_pose.header = header
            msg_pose.pose.position = msg_location.point
            msg_pose.pose.position.z = ground_distance
            msg_pose.pose.orientation = msg_attitude.quaternion
            self.pub_pose.publish(msg_pose)

            Rot = R.from_quat([
                drone_quat['x'], -drone_quat['y'], -drone_quat['z'],
                drone_quat['w']
            ])
            drone_rpy = Rot.as_euler('xyz')

            msg_odometry = Odometry()
            msg_odometry.header = header
            msg_odometry.child_frame_id = '/body'
            msg_odometry.pose.pose = msg_pose.pose
            msg_odometry.twist.twist.linear.x = math.cos(
                drone_rpy[2]) * msg_speed.vector.x + math.sin(
                    drone_rpy[2]) * msg_speed.vector.y
            msg_odometry.twist.twist.linear.y = -math.sin(
                drone_rpy[2]) * msg_speed.vector.x + math.cos(
                    drone_rpy[2]) * msg_speed.vector.y
            msg_odometry.twist.twist.linear.z = msg_speed.vector.z
            self.pub_odometry.publish(msg_odometry)

            # log battery percentage
            if battery_percentage >= 30:
                if battery_percentage % 10 == 0:
                    rospy.loginfo_throttle(
                        100, "Battery level: " + str(battery_percentage) + "%")
            else:
                if battery_percentage >= 20:
                    rospy.logwarn_throttle(
                        10, "Low battery: " + str(battery_percentage) + "%")
                else:
                    if battery_percentage >= 10:
                        rospy.logerr_throttle(
                            1, "Critical battery: " + str(battery_percentage) +
                            "%")
                    else:
                        rospy.logfatal_throttle(
                            0.1,
                            "Empty battery: " + str(battery_percentage) + "%")

            # log signal strength
            if wifi_rssi <= -60:
                if wifi_rssi >= -70:
                    rospy.loginfo_throttle(
                        100, "Signal strength: " + str(wifi_rssi) + "dBm")
                else:
                    if wifi_rssi >= -80:
                        rospy.logwarn_throttle(
                            10, "Weak signal: " + str(wifi_rssi) + "dBm")
                    else:
                        if wifi_rssi >= -90:
                            rospy.logerr_throttle(
                                1,
                                "Unreliable signal:" + str(wifi_rssi) + "dBm")
                        else:
                            rospy.logfatal_throttle(
                                0.1,
                                "Unusable signal: " + str(wifi_rssi) + "dBm")
        else:
            rospy.logwarn("Packet lost!")