我们从Python开源项目中，提取了以下50个代码示例，用于说明如何使用rospy.get_time()。
def talker(): pub = rospy.Publisher('chatter', String, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) pub.publish(hello_str) rate.sleep() # rate = rospy.Rate(10) # hello_str = "hello world" # rospy.loginfo(hello_str) # pub.publish(hello_str) # rospy.spin() # exit(0)
def __init__(self): self._read_configuration() if self.show_plots: self._setup_plots() rospy.loginfo("Receiving timestamp messages from {}".format(self.uwb_timestamps_topic)) rospy.loginfo("Publishing multi-range messages to {}".format(self.uwb_multi_range_topic)) rospy.loginfo("Publishing raw multi-range messages to {}".format(self.uwb_multi_range_raw_topic)) rospy.loginfo("Publishing multi-range-with-offsets messages to {}".format( self.uwb_multi_range_with_offsets_topic)) # ROS Publishers self.uwb_pub = rospy.Publisher(self.uwb_multi_range_topic, uwb.msg.UWBMultiRange, queue_size=1) self.uwb_raw_pub = rospy.Publisher(self.uwb_multi_range_raw_topic, uwb.msg.UWBMultiRange, queue_size=1) self.uwb_with_offsets_pub = rospy.Publisher(self.uwb_multi_range_with_offsets_topic, uwb.msg.UWBMultiRangeWithOffsets, queue_size=1) self.uwb_timestamps_sub = rospy.Subscriber(self.uwb_timestamps_topic, uwb.msg.UWBMultiRangeTimestamps, self.handle_timestamps_message) # Variables for rate display self.msg_count = 0 self.last_now = rospy.get_time()
def initialize_estimate(self, estimate_id, initial_state): """Initialize a state estimate with identity covariance. The initial estimate is saved in the `self.estimates` dictionary. The timestamp in the `self.estimate_times` is updated. Args: estimate_id (int): ID of the tracked target. initial_state (int): Initial state of the estimate. Returns: X (numpy.ndarray): Solution of equation. """ x = initial_state P = self.initial_position_covariance * np.eye(6) P[3:6, 3:6] = 0 estimate = UWBTracker.StateEstimate(x, P) self.estimates[estimate_id] = estimate self.estimate_times[estimate_id] = rospy.get_time() self.ikf_prev_outlier_flags[estimate_id] = False self.ikf_outlier_counts[estimate_id] = 0
def wifi_corrections_callback(self, msg): # Number of corrections received. self.number_corrections_wifi_status['text'] = str(msg.received_corrections) # Compute rate corrections. width_time_window = rospy.get_time() - self.time_first_sample_moving_window if width_time_window >= wifiCorrectionsHzAverage: samples_in_time_window = msg.received_corrections - self.num_corrections_first_sample_moving_window average_corrections_hz = samples_in_time_window / width_time_window self.hz_corrections_wifi_status['text'] = str(round(average_corrections_hz, 1)) # Reset and start again. self.num_corrections_first_sample_moving_window = msg.received_corrections self.time_first_sample_moving_window = rospy.get_time() # Ping base station. self.ping_corrections_wifi_status['text'] = str(round(msg.latency, 2))
def main(): rospy.init_node('easy_handeye') while rospy.get_time() == 0.0: pass print('Handeye Calibration Commander') print('connecting to: {}'.format((rospy.get_namespace().strip('/')))) cmder = HandeyeCalibrationCommander() if cmder.client.eye_on_hand: print('eye-on-hand calibration') print('robot effector frame: {}'.format(cmder.client.robot_effector_frame)) else: print('eye-on-base calibration') print('robot base frame: {}'.format(cmder.client.robot_base_frame)) print('tracking base frame: {}'.format(cmder.client.tracking_base_frame)) print('tracking target frame: {}'.format(cmder.client.tracking_marker_frame)) cmder.spin_interactive()
def main(): rospy.init_node('easy_handeye') while rospy.get_time() == 0.0: pass cw = HandeyeServer() rospy.spin()
def wobble(self): self.set_neutral() """ Performs the wobbling """ command_rate = rospy.Rate(1) control_rate = rospy.Rate(100) start = rospy.get_time() while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0): angle = random.uniform(-2.0, 0.95) while (not rospy.is_shutdown() and not (abs(self._head.pan() - angle) <= intera_interface.HEAD_PAN_ANGLE_TOLERANCE)): self._head.set_pan(angle, speed=0.3, timeout=0) control_rate.sleep() command_rate.sleep() self._done = True rospy.signal_shutdown("Example finished.")
def compute_output(self, error): """ Performs a PID computation and returns a control value based on the elapsed time (dt) and the error signal from a summing junction (the error parameter). """ self._cur_time = rospy.get_time() # get t dt = self._cur_time - self._prev_time # get delta t de = error - self._prev_err # get delta error self._cp = error # proportional term self._ci += error * dt # integral term self._cd = 0 if dt > 0: # no div by zero self._cd = de / dt # derivative term self._prev_time = self._cur_time # save t for next pass self._prev_err = error # save t-1 error # sum the terms and return the result return ((self._kp * self._cp) + (self._ki * self._ci) + (self._kd * self._cd))
def _select_play(self): if self._play_termination: # ????Role????? for role in self._play.roles: role.behavior.reset() # Extract possible plays from playbook possible_plays = [] for play in PlayBook.book: if WorldModel.situations[play.applicable]: possible_plays.append(play) # TODO(Asit) select a play randomly if possible_plays: self._play = possible_plays[0] else: self._play = PlayDummy() self._play_past_time = rospy.get_time() self._play_termination = False rospy.loginfo('play reset')
def _evaluate_play(self): for role in self._play.roles: status = role.behavior.get_status() if role.loop_enable: if status == TaskStatus.SUCCESS or status == TaskStatus.FAILURE: role.behavior.reset() else: if status == TaskStatus.SUCCESS or status == TaskStatus.FAILURE: self._play_termination = True if self._play.timeout: if rospy.get_time() - self._play_past_time > self._play.timeout: self._play_termination = True # TODO(Asit) write recent_done termination if (self._play.done and WorldModel.situations[self._play.done]) or \ (self._play.done_aborted and not WorldModel.situations[self._play.done_aborted]): self._play_termination = True
def handle_wait_event(self, goal): d = datetime.datetime.fromtimestamp(rospy.get_time()) query_result_count = 0; while query_result_count == 0 and self.wait_for_event_server.is_active() == True: if self.wait_for_event_server.is_preempt_requested(): result = WaitForEventResult() result.result = False result.error_msg = 'The client cancel goal.' self.wait_for_event_server.set_preempted(result) return result for i in range(len(goal.event_name)): memory_name = goal.event_name[i] memory_query = json.loads(goal.query[i]) memory_query['time'] = {"$gte": d} query_result = self.collector[memory_name].find(memory_query) query_result_count += query_result.count() rospy.sleep(0.2) result = WaitEventResult() result.result = True self.wait_for_event_server.set_succeeded(result)
def rGripperForceCallback(self, msg): RGripRFingerForceRaw = np.array(msg.r_finger_tip) if self.zeroing and self.RGripRFingerForceMean is None: # Determine a mean for readings to zero out data self.RGripRFingerForceRecent.append(RGripRFingerForceRaw) if len(self.RGripRFingerForceRecent) >= 20: self.RGripRFingerForceMean = np.mean(self.RGripRFingerForceRecent, axis=0) elif self.RGripRFingerForceMean is not None: # Process the raw electronic readings into newton force and kilopascal pressure readings # Originates from /opt/ros/indigo/lib/python2.7/dist-packages/fingertip_pressure/fingertip_panel.py self.RGripRFingerForce = (RGripRFingerForceRaw - self.RGripRFingerForceMean) / self.forcePerUnit self.RGripRFingerPressure = self.RGripRFingerForce / self.tactileAreas / 1000.0 RGripRFingerPressureRaw = (RGripRFingerForceRaw / self.forcePerUnit) / self.tactileAreas / 1000.0 if self.recording: # Record data self.dataAll['RGripRFingerTime'][-1].append(rospy.get_time() - self.startTime) self.dataAll['RGripRFingerForceRaw'][-1].append(RGripRFingerForceRaw) self.dataAll['RGripRFingerForce'][-1].append(self.RGripRFingerForce) self.dataAll['RGripRFingerPressure'][-1].append(self.RGripRFingerPressure) self.dataAll['RGripRFingerPressureRaw'][-1].append(RGripRFingerPressureRaw)
def save_recipe_dp(self, variable): """ Save the recipe start/end to the env. data pt. DB, so we can restart the recipe if necessary. """ doc = EnvironmentalDataPoint({ "environment": self.environment, "variable": variable, "is_desired": True, "value": rospy.get_param(params.CURRENT_RECIPE), "timestamp": rospy.get_time() }) doc_id = gen_doc_id(rospy.get_time()) self.env_data_db[doc_id] = doc #------------------------------------------------------------------------------ # Our ROS node main entry point. Starts up the node and then waits forever.
def receive_behavior(self, msg): """ Gets an Int based on the status of the robot """ self.behavior = msg.data if self.walk_failure is not None: return if self.start_walk is not None: if self.behavior == 4: log("Walk started") self.start_walk = None if self.last_footstep is None: self.last_footstep = rospy.get_time() else: now = rospy.get_time() if now > self.start_walk + 10.0: self.walk_failure = 'WALK FAILED: Asked to walk, and ' \ 'never started' log(self.walk_failure) if self.last_footstep is not None: now = rospy.get_time() if now > self.last_footstep + 10.0: self.walk_failure = 'WALK FAILED: footstep {} is taking over ' \ '10 seconds to complete'.format(self.steps) log(self.walk_failure)
def forward(self, distance, square_up=True, start_foot=LEFT): """ Walk forward the given distance """ msg = FootstepDataListRosMessage() msg.default_transfer_time = self.transfer_time msg.default_swing_time = self.swing_time stride = self.stride if distance < 0 and stride > 0.3: log('Walking backward exceeds safe stride') stride = 0.3 msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = self.zarj.uid # TODO - this may be nice to rethink self.lookup_feet(report=True) msg.footstep_data_list = self.create_footsteps(distance, start_foot, square_up, self.stride) self.steps = 0 self.planned_steps = len(msg.footstep_data_list) self.walk_failure = None self.start_walk = rospy.get_time() self.footstep_list_publisher.publish(msg) log('Move ' + fmt(distance) + ' FootstepDataList: uid ' + str(msg.unique_id))
def walk_to(self, point): """ Shuffle to a given point, point is given for the left foot """ msg = FootstepDataListRosMessage() msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME msg.default_swing_time = self.DEFAULT_SWING_TIME msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = self.zarj.uid self.lookup_feet() delta = Vector3(point.x - self.lf_start_position.x, point.y - self.lf_start_position.y, 0) start_foot = self._find_first_xy_foot(point) msg.footstep_data_list = self.create_xy_steps(delta, start_foot, 0.15) self.steps = 0 self.planned_steps = len(msg.footstep_data_list) self.walk_failure = None self.start_walk = rospy.get_time() self.footstep_list_publisher.publish(msg) log('FootstepDataList: uid ' + str(msg.unique_id))
def step_up(self, distance, rise): """ Step up a step """ msg = FootstepDataListRosMessage() msg.default_transfer_time = self.transfer_time msg.default_swing_time = self.swing_time msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = self.zarj.uid self.lookup_feet() start_foot = FootstepDataRosMessage.LEFT msg.footstep_data_list = self.create_up_steps(distance, rise, start_foot, self.DEFAULT_STRIDE) self.steps = 0 self.planned_steps = len(msg.footstep_data_list) self.walk_failure = None self.start_walk = rospy.get_time() self.footstep_list_publisher.publish(msg) log('FootstepDataList: uid ' + str(msg.unique_id))
def send_stereo_camera(self): # Black and white image is about 225K # That should consume about 2 seconds worth of bandwidth; hopefully be okay self.cloud = self.zarj.eyes.get_stereo_cloud() img, self.img_details = self.zarj.eyes.get_cloud_image_with_details(self.cloud) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) (_, png) = cv2.imencode(".png", gray) picturemsg = ZarjPicture("Image of satellite hands", png, True) picturemsg.time = rospy.get_time() self.points = [ None, None ] self.zarj_comm.push_message(picturemsg)
def calc_interpolation(self, previous_goalpoint, next_goalpoint, t_prev, t_next): """ interpolate cartesian positions (x,y,z) between last goalpoint and previous goalpoint at the current time :param previous_goalpoint: :param next_goalpoint: :param goto_point: :param tnewpos: :return: des_pos """ assert (rospy.get_time() >= t_prev) des_pos = previous_goalpoint + (next_goalpoint - previous_goalpoint) * (rospy.get_time()- t_prev)/ (t_next - t_prev) if rospy.get_time() >= t_next: des_pos = next_goalpoint print 't > tnext' # print 'current_delta_time: ', self.curr_delta_time # print "interpolated pos:", des_pos return des_pos
def save(self, i_save, action, endeffector_pose): self.t_savereq = rospy.get_time() assert self.instance_type == 'main' if self.use_aux: # request save at auxiliary recorders try: rospy.wait_for_service('get_kinectdata', 0.1) resp1 = self.save_kinectdata_func(i_save) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) raise ValueError('get_kinectdata service failed') if self.save_images: self._save_img_local(i_save) if self.save_actions: self._save_state_actions(i_save, action, endeffector_pose) if self.save_gif: highres = cv2.cvtColor(self.ltob.img_cv2, cv2.COLOR_BGR2RGB) print 'highres dim',highres.shape self.highres_imglist.append(highres)
def _execute_gripper_commands(self): start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec() r_cmd = self._r_grip.trajectory.points l_cmd = self._l_grip.trajectory.points pnt_times = [pnt.time_from_start.to_sec() for pnt in r_cmd] end_time = pnt_times[-1] rate = rospy.Rate(self._gripper_rate) now_from_start = rospy.get_time() - start_time while(now_from_start < end_time + (1.0 / self._gripper_rate) and not rospy.is_shutdown()): idx = bisect(pnt_times, now_from_start) - 1 if self._r_gripper.type() != 'custom': self._r_gripper.command_position(r_cmd[idx].positions[0]) if self._l_gripper.type() != 'custom': self._l_gripper.command_position(l_cmd[idx].positions[0]) rate.sleep() now_from_start = rospy.get_time() - start_time
def callback(self, msg): """ ROS Callback for new diagnostic messages Puts new msg into the queue, and emits a signal to let listeners know that the timeline has been updated If the timeline is paused, new messages are placed into a separate queue and swapped back in when the timeline is unpaused :type msg: Either DiagnosticArray or DiagnosticsStatus. Can be determined by __init__'s arg "msg_callback". """ self._have_messages = True self._last_message_time = rospy.get_time() if self.paused: self._paused_queue.append(msg) else: self._queue.append(msg) self._queue_copy = copy.deepcopy(self._queue) self.queue_updated.emit(self._queue_copy) self.message_updated.emit(msg)
def callback(data): """Callback function for subscribing to an Image topic and creating a buffer """ global dtype global images_so_far # Get cv image (which is a numpy array) from data cv_image = bridge.imgmsg_to_cv2(data) # Save dtype before we float32-ify it dtype = str(cv_image.dtype) # Insert and roll buffer buffer.insert(0, (np.asarray(cv_image, dtype='float32'), rospy.get_time())) if(len(buffer) > bufsize): buffer.pop() # for showing framerate images_so_far += 1 # Initialize subscriber with our callback
def callback(data,pub): t = Twist() if data.data == 1: t.linear.x = .25 print t if data.data == 2: t.angular.z = 2 if data.data == 3: t.linear.x = .25 t.angular.z = 1 time = rospy.get_time() while rospy.get_time()-time < 6: pub.publish (t) rospy.sleep(.005)
def update_estimate(self, multi_range_msg): """Update tracker based on a multi-range message. Updates estimate and timestamp in the `self.estimate` and `self.estimate_times` dictionaries. Args: multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. Returns: new_estimate (StateEstimate): Updated position estimate. """ estimate_id = (multi_range_msg.address, multi_range_msg.remote_address) if estimate_id not in self.estimates: initial_state = self.initial_guess(multi_range_msg.ranges) if initial_state is None: return None self.initialize_estimate(estimate_id, initial_state) current_time = rospy.get_time() timestep = current_time - self.estimate_times[estimate_id] estimate = self.estimates[estimate_id] new_estimate, outlier_flag = self.update_filter(timestep, estimate, multi_range_msg.ranges) if not outlier_flag: self.estimates[estimate_id] = new_estimate self.estimate_times[estimate_id] = current_time if self.ikf_prev_outlier_flags[estimate_id]: self.ikf_prev_outlier_flags[estimate_id] = False # If too many outliers are encountered in a row the estimate is deleted. # This will lead to a new initial guess for the next multi-range message. if outlier_flag: if not self.ikf_prev_outlier_flags[estimate_id]: self.ikf_prev_outlier_flags[estimate_id] = True self.ikf_outlier_counts[estimate_id] = 0 self.ikf_outlier_counts[estimate_id] += 1 if self.ikf_outlier_counts[estimate_id] >= self.ikf_max_outlier_count: del self.estimates[estimate_id] rospy.loginfo('Too many outliers in a row. Resetting estimate for address={}, remote_address={}'.format( multi_range_msg.address, multi_range_msg.remote_address )) return new_estimate
def main(): rospy.init_node("talker") pub = rospy.Publisher("/chatter_topic", String, queue_size=1) rate = rospy.Rate(10) # 10 Hertz ile çal???yor while not rospy.is_shutdown(): message = "Naber Dünya? %s" % (rospy.get_time()) rospy.loginfo("Mesaj haz?rland?: %s" % message) pub.publish(message) rate.sleep()
def _execute_gripper_commands(self): start_time = rospy.get_time() - self._trajectory_actual_offset.to_sec() grip_cmd = self.grip.trajectory.points pnt_times = [pnt.time_from_start.to_sec() for pnt in grip_cmd] end_time = pnt_times[-1] rate = rospy.Rate(self._gripper_rate) now_from_start = rospy.get_time() - start_time while(now_from_start < end_time + (1.0 / self._gripper_rate) and not rospy.is_shutdown()): idx = bisect(pnt_times, now_from_start) - 1 if self.gripper: self.gripper.set_position(grip_cmd[idx].positions[0]) rate.sleep() now_from_start = rospy.get_time() - start_time
def __init__(self, filename, rate, side="right"): """ Records joint data to a file at a specified rate. """ self.gripper_name = '_'.join([side, 'gripper']) self._filename = filename self._raw_rate = rate self._rate = rospy.Rate(rate) self._start_time = rospy.get_time() self._done = False self._limb_right = intera_interface.Limb(side) try: self._gripper = intera_interface.Gripper(side) rospy.loginfo("Electric gripper detected.") except Exception as e: self._gripper = None rospy.loginfo("No electric gripper detected.") # Verify Gripper Have No Errors and are Calibrated if self._gripper: if self._gripper.has_error(): self._gripper.reboot() if not self._gripper.is_calibrated(): self._gripper.calibrate() self._cuff = intera_interface.Cuff(side)
def _time_stamp(self): return rospy.get_time() - self._start_time
def _update_feedback(self, cmd_point, jnt_names, cur_time): self._fdbk.header.stamp = rospy.Duration.from_sec(rospy.get_time()) self._fdbk.joint_names = jnt_names self._fdbk.desired = cmd_point self._fdbk.desired.time_from_start = rospy.Duration.from_sec(cur_time) self._fdbk.actual.positions = self._get_current_position(jnt_names) self._fdbk.actual.time_from_start = rospy.Duration.from_sec(cur_time) self._fdbk.error.positions = map(operator.sub, self._fdbk.desired.positions, self._fdbk.actual.positions ) self._fdbk.error.time_from_start = rospy.Duration.from_sec(cur_time) self._server.publish_feedback(self._fdbk)
def wait_for(test, timeout=1.0, raise_on_error=True, rate=100, timeout_msg="timeout expired", body=None): """ Waits until some condition evaluates to true. @param test: zero param function to be evaluated @param timeout: max amount of time to wait. negative/inf for indefinitely @param raise_on_error: raise or just return False @param rate: the rate at which to check @param timout_msg: message to supply to the timeout exception @param body: optional function to execute while waiting """ end_time = rospy.get_time() + timeout rate = rospy.Rate(rate) notimeout = (timeout < 0.0) or timeout == float("inf") while not test(): if rospy.is_shutdown(): if raise_on_error: raise OSError(errno.ESHUTDOWN, "ROS Shutdown") return False elif (not notimeout) and (rospy.get_time() >= end_time): if raise_on_error: raise OSError(errno.ETIMEDOUT, timeout_msg) return False if callable(body): body() rate.sleep() return True
def talker(): hello_str = message + " %s" % rospy.get_time() rospy.loginfo(hello_str) return hello_str
def handle_write_data(self, req): recv_data = json.loads(req.data) write_data = recv_data.copy() write_data['time'] = datetime.datetime.fromtimestamp(rospy.get_time()) write_data['by'] = req.by if rospy.get_name() == '/environmental_memory' and req.perception_name != 'objects_info': self.collections[req.perception_name].update_one( {'name':write_data['name']}, {'$set': write_data}, upsert=True) else: self.collections[req.perception_name].insert_one(write_data) return WriteDataResponse(True)
def accelerometerCallback(self, msg): accelAllRaw = [[s.x, s.y, s.z] for s in msg.samples] accelRaw = np.mean(accelAllRaw, axis=0) if self.zeroing and self.accelMean is None: # Determine a mean for readings to zero out data self.accelRecent.append(accelRaw) if len(self.accelRecent) >= 20: self.accelMean = np.mean(self.accelRecent, axis=0) elif self.recording and self.accelMean is not None: # Record data self.dataAll['accelerometerTime'][-1].extend([rospy.get_time() - self.startTime]*len(accelAllRaw)) self.dataAll['accelerometerRaw'][-1].extend(accelAllRaw) self.dataAll['accelerometer'][-1].extend([np.array(a) - self.accelMean for a in accelAllRaw])
def get_state(self): """ Get the state-related variables of the currently running recipe """ now_time = rospy.get_time() start_time = self.__start_time or now_time return self.get_recipe(), start_time, now_time
def set_recipe(self, recipe, start_time): """ Set the currently running recipe... this is the CouchDB recipe document. """ with self.lock: if self.__recipe is not None: raise RecipeRunningError("Recipe is already running") # Set recipe and time self.__recipe = recipe self.__start_time = start_time if self.__start_time is None: self.__start_time = rospy.get_time() rospy.set_param(params.CURRENT_RECIPE, recipe["_id"]) rospy.set_param(params.CURRENT_RECIPE_START, self.__start_time ) return self
def ros_next(rate_hz): ros_next.prev_time = rospy.get_time() timeout = 1 / rate_hz def closure(): curr_time = rospy.get_time() if curr_time - ros_next.prev_time > timeout: ros_next.prev_time = curr_time return True else: return False return closure # Read and verify the serial message string.
def ros_next(rate_hz): ros_next.prev_time = rospy.get_time() timeout = 1 / rate_hz def closure(): curr_time = rospy.get_time() if curr_time - ros_next.prev_time > timeout: ros_next.prev_time = curr_time return True else: return False return closure
def debug_sequence(): """ debug sequence """ return rospy.get_time()
def receive_footstep_status(self, msg): """ Get our current footstep status """ if msg.status == 1: self.steps += 1 log('Completed step: {} of {}; total received {}.'.format( msg.footstep_index, self.planned_steps, self.steps)) if self.steps < self.planned_steps: self.last_footstep = rospy.get_time() else: self.last_footstep = None
def move_foot(self, foot_type, x, y, rotate): """ Move a foot, relative to the current left foot frame and rotate by a particular angle. This code is meant to be used for setting up messy test scenarios that can be fixed by the *turn* method. """ self.lookup_feet(report = True) left_ft_angle = compute_rot_angle(self.lf_start_orientation) left_ft_rot = quaternion_about_axis(radians(left_ft_angle), [0, 0, 1]) offset = rotate_v([x, y ,0], left_ft_rot) if foot_type == "left": ft_loc = msg_v_to_v(self.lf_start_position) foot_choice = LEFT else: ft_loc = msg_v_to_v(self.rf_start_position) foot_choice = RIGHT target = add_v(ft_loc, offset) foot_rotation = quaternion_about_axis(radians(left_ft_angle + rotate), [0, 0, 1]) msg = FootstepDataListRosMessage() msg.footstep_data_list = [self.create_one_abs_footstep(foot_choice, v_to_msg_v(target), q_to_msg_q(foot_rotation))] msg.default_transfer_time = self.transfer_time msg.default_swing_time = self.swing_time msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = self.zarj.uid self.steps = 0 self.planned_steps = len(msg.footstep_data_list) self.walk_failure = None self.start_walk = rospy.get_time() self.footstep_list_publisher.publish(msg) log("Moving {} foot by x={}, y={}, and rotating to {}".format( foot_type, fmt(x), fmt(y), fmt(left_ft_angle + rotate))) while not self.walk_is_done(): rospy.sleep(0.2) rospy.sleep(0.2)
def fix_stance(self): """ fix stance """ self.lookup_feet(report=True) distance_between_feet = sqrt( ( self.rf_start_position.x - self.lf_start_position.x )**2 + ( self.rf_start_position.y - self.lf_start_position.y )**2) if abs(distance_between_feet - self.stance_width) > 0.01: distance = self.stance_width - distance_between_feet else: return msg = FootstepDataListRosMessage() msg.default_transfer_time = self.DEFAULT_TRANSFER_TIME msg.default_swing_time = self.DEFAULT_SWING_TIME msg.execution_mode = msg.OVERRIDE # Override means replace others msg.unique_id = self.zarj.uid start_foot = FootstepDataRosMessage.LEFT msg.footstep_data_list = [self.create_one_footstep(start_foot, [0.0, distance, 0.0])] self.steps = 0 self.planned_steps = len(msg.footstep_data_list) self.walk_failure = None self.start_walk = rospy.get_time() self.footstep_list_publisher.publish(msg) log('FootstepDataList: uid ' + str(msg.unique_id))
def run(self): """ Run our code """ #print("Start looking for a harness") sub = rospy.Subscriber( "/srcsim/finals/harness", Harness, self.recv_harness) rate = rospy.Rate(10) # 10hz for i in range(10): if sub.get_num_connections() == 0: rate.sleep() if sub.get_num_connections() == 0: sub.unregister() #print("No connection found; not harnessed.") return 1 rospy.sleep(0.5) # Okay, if we did not get a harness message at all, # then let's chill for a while. If we still get nothing, # assume that we missed the '0' message if self.harness is None: rospy.sleep(2.0) sub.unregister() if self.harness == 0: print("Actually heard the harnessed message!") return 0 if self.harness is None: print("Connected, and no harness messages; assuming harnessed at {}".format(rospy.get_time())) return 0 #print("Harness currently {} at {}".format(self.harness, rospy.get_time())) return 2
def send_left_camera(self): pictures = self.zarj.eyes.get_images() # TODO - 320x240 color image is about 135K bytes # Black and white 544x1024 is ~300K bytes # Black and white 272x512 is ~85K bytes # Max for task 1 is 50k *bits* per second # Max for tasks 2/3 is 1M bits per second pictsize = np.shape(pictures[0]) resized = cv2.resize(pictures[0], (pictsize[1]/2,pictsize[0]/2), interpolation = cv2.INTER_AREA) gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY) (_, png) = cv2.imencode(".png", gray) msg = ZarjPicture("lefteye", png) msg.time = rospy.get_time() self.zarj_comm.push_message(msg)
def move_with_impedance_sec(self, cmd, tsec = 2.): """ blocking """ tstart = rospy.get_time() delta_t = 0 while delta_t < tsec: delta_t = rospy.get_time() - tstart self.move_with_impedance(cmd)
def __init__(self, limb = "right"): # control parameters self._rate = 1000.0 # Hz self._missed_cmds = 20.0 # Missed cycles before triggering timeout # create our limb instance self._limb = intera_interface.Limb(limb) # initialize parameters self._springs = dict() self._damping = dict() self._des_angles = dict() # create cuff disable publisher cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction' self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1) # verify robot is enabled print("Getting robot state... ") self._rs = intera_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Running. Ctrl-c to quit") rospy.Subscriber("desired_joint_pos", JointState, self._set_des_pos) rospy.Subscriber("release_spring", Float32, self._release) rospy.Subscriber("imp_ctrl_active", Int64, self._imp_ctrl_active) self.max_stiffness = 20 self.time_to_maxstiffness = .3 ######### 0.68 self.t_release = rospy.get_time() self._imp_ctrl_is_active = True for joint in self._limb.joint_names(): self._springs[joint] = 30 self._damping[joint] = 4
