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)
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()
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!')
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
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)
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})
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)
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
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()
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!')
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)
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)
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())
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!")