我们从Python开源项目中，提取了以下40个代码示例，用于说明如何使用rospy.get_rostime()。
def _to_time_inst(msg, rostype, inst=None): # Create an instance if we haven't been provided with one if rostype == "time" and msg == "now": return rospy.get_rostime() if inst is None: if rostype == "time": inst = rospy.rostime.Time() elif rostype == "duration": inst = rospy.rostime.Duration() else: return None # Copy across the fields for field in ["secs", "nsecs"]: try: if field in msg: setattr(inst, field, msg[field]) except TypeError: continue return inst
def init_transforms(self): """ Initialize the tf2 transforms """ rospy.loginfo("Start transform calibration.") self.tf_buffer = tf2_ros.Buffer(rospy.Duration(300.0)) self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) while True: try: now = rospy.get_rostime() self.tf_buffer.lookup_transform('head', 'left_camera_optical_frame', now - rospy.Duration(0.1)) except: self.rate.sleep() continue break rospy.loginfo('transform calibration finished.')
def create_image_from_cloud(self, points): size = 100, 100, 1 img = numpy.zeros(size, numpy.uint8) #tf = self.zarj_os.transform.lookup_transform('world', 'head', rospy.get_rostime()) print points[0] #print self.zarj_os.transform.transform_from_world(points[0]) for p in points: #tp = self.zarj_os.transform.transform_from_world(p) ipx = int((p.x * 100.0) + 50.0) ipy = int((p.y * 100.0) + 50.0) if ipx >= 0 and ipx < 100 and ipy >= 0 and ipy < 100: ipz = int(p.z * 100.0) if ipz > 255: ipz = 255 img[ipx, ipy] = ipz return img
def send_status(self): """ Run our code """ next_status_send = rospy.get_rostime().to_sec() next_picture_send = rospy.get_rostime().to_sec() while True: rospy.sleep(0.1) if not self.zarj_comm.connected: continue now = rospy.get_rostime().to_sec() if now >= next_status_send: next_status_send = now + self.STATUS_INTERVAL zmsg = ZarjStatus(self.task, self.checkpoint, self.elapsed_time, now, self.score, self.harness) self.zarj_comm.push_message(zmsg) if self.picture_interval > 0.01 and now >= next_picture_send: next_picture_send = now + self.picture_interval self.send_left_camera() if self.task >= 3: self.send_lidar(True)
def get_data(self, arm=TRIAL_ARM): """ Request for the most recent value for data/sensor readings. Returns entire sample report (all available data) in sample. Args: arm: TRIAL_ARM or AUXILIARY_ARM. """ request = DataRequest() request.id = self._get_next_seq_id() request.arm = arm request.stamp = rospy.get_rostime() result_msg = self._data_service.publish_and_wait(request) # TODO - Make IDs match, assert that they match elsewhere here. sample = msg_to_sample(result_msg, self) return sample # TODO - The following could be more general by being relax_actuator # and reset_actuator.
def handle(req): global msg, pub, task print 'get resp class' response_class = get_response_class(req) print 'resp', response_class try: task = req with task_change_lock: pub, msg = get_message_publisher(task) print pub, msg msg.header.stamp = rospy.get_rostime() pub.publish(msg) if not offboard_and_arm(): return response_class(success=False, message='Cannot arm or offboard the vehicle') return response_class(success=True) except Exception as e: return response_class(success=False, message=e.message)
def _check_battery_state(_battery_acpi_path): """ @return LaptopChargeStatus """ o = slerp(_battery_acpi_path+'/state') batt_info = yaml.load(o) rv = LaptopChargeStatus() state = batt_info.get('charging state', 'discharging') rv.charge_state = state_to_val.get(state, 0) rv.rate = _strip_A(batt_info.get('present rate', '-1 mA')) if rv.charge_state == LaptopChargeStatus.DISCHARGING: rv.rate = math.copysign(rv.rate, -1) # Need to set discharging rate to negative rv.charge = _strip_Ah(batt_info.get('remaining capacity', '-1 mAh')) rv.voltage = _strip_V(batt_info.get('present voltage', '-1 mV')) rv.present = batt_info.get('present', False) rv.header.stamp = rospy.get_rostime() return rv
def __init__(self): self.charging_state = {0:"Not Charging", 1:"Reconditioning Charging", 2:"Full Charging", 3:"Trickle Charging", 4:"Waiting", 5:"Charging Fault Condition"} self.charging_source = {0:"None", 1:"Internal Charger", 2:"Base Dock"} self.digital_outputs = {0:"OFF", 1:"ON"} self.oi_mode = {1:"Passive", 2:"Safe", 3:"Full"} self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray) self.last_diagnostics_time = rospy.get_rostime()
def publish_position(self, pos, ps_pub, ps_cov_pub, cov): x, y = pos[0], pos[1] if len(pos) > 2: z = pos[2] else: z = 0 ps = PoseStamped() ps_cov = PoseWithCovarianceStamped() ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z ps.header.frame_id = self.frame_id ps.header.stamp = rospy.get_rostime() ps_cov.header = ps.header ps_cov.pose.pose = ps.pose ps_cov.pose.covariance = cov ps_pub.publish(ps) ps_cov_pub.publish(ps_cov)
def __get_point_msg(cls, data, frame): """ Deserializes a location to a message of type PointStamped. Args: data: A dictionary containing the location. frame: Frame for the point. Returns: A message of type PointStamped with the location. """ header = Header() header.stamp = rospy.get_rostime() header.frame_id = frame msg = GeoPointStamped() msg.header = header msg.position.latitude = data["latitude"] msg.position.longitude = data["longitude"] return msg
def cmdVel_publish(self, cmdVelocity): # Publish Twist self.cmdVel_pub.publish(cmdVelocity) # Publish TwistStamped self.baseVelocity.twist = cmdVelocity baseVelocity = TwistStamped() baseVelocity.twist = cmdVelocity now = rospy.get_rostime() baseVelocity.header.stamp.secs = now.secs baseVelocity.header.stamp.nsecs = now.nsecs self.cmdVelStamped_pub.publish(baseVelocity)
def IK(self, T_goal): req = moveit_msgs.srv.GetPositionIKRequest() req.ik_request.group_name = self.group_name req.ik_request.robot_state = moveit_msgs.msg.RobotState() req.ik_request.robot_state.joint_state = self.joint_state req.ik_request.avoid_collisions = True req.ik_request.pose_stamped = geometry_msgs.msg.PoseStamped() req.ik_request.pose_stamped.header.frame_id = "world_link" req.ik_request.pose_stamped.header.stamp = rospy.get_rostime() req.ik_request.pose_stamped.pose = convert_to_message(T_goal) req.ik_request.timeout = rospy.Duration(3.0) res = self.ik_service(req) q = [] if res.error_code.val == res.error_code.SUCCESS: q = self.q_from_joint_state(res.solution.joint_state) return q
def handle_multi_range_message(self, multi_range_msg): """Handle a ROS multi-range message by updating and publishing the state. Args: multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message. """ # Update tracker position based on time-of-flight measurements new_estimate = self.update_estimate(multi_range_msg) if new_estimate is None: rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format( multi_range_msg.address, multi_range_msg.remote_address)) else: # Publish tracker message ros_msg = uwb.msg.UWBTracker() ros_msg.header.stamp = rospy.get_rostime() ros_msg.address = multi_range_msg.address ros_msg.remote_address = multi_range_msg.remote_address ros_msg.state = new_estimate.state ros_msg.covariance = np.ravel(new_estimate.covariance) self.uwb_pub.publish(ros_msg) # Publish target transform (rotation is identity) self.tf_broadcaster.sendTransform( (new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.get_rostime(), self.target_frame, self.tracker_frame )
def _to_object_inst(msg, rostype, roottype, inst, stack): # Typecheck the msg if type(msg) is not dict: raise FieldTypeMismatchException(roottype, stack, rostype, type(msg)) # Substitute the correct time if we're an std_msgs/Header if rostype in ros_header_types: inst.stamp = rospy.get_rostime() inst_fields = dict(zip(inst.__slots__, inst._slot_types)) for field_name in msg: # Add this field to the field stack field_stack = stack + [field_name] # Raise an exception if the msg contains a bad field if not field_name in inst_fields: raise NonexistentFieldException(roottype, field_stack) field_rostype = inst_fields[field_name] field_inst = getattr(inst, field_name) field_value = _to_inst(msg[field_name], field_rostype, roottype, field_inst, field_stack) setattr(inst, field_name, field_value) return inst
def run(self): """ Run our code """ rospy.loginfo("Start laser test code") rospy.wait_for_service("assemble_scans2") # Todo - publish the spin logic... self.laser_publisher = rospy.Publisher("/multisense/lidar_cloud", PointCloud, queue_size=10) self.scan_service = rospy.ServiceProxy('assemble_scans', AssembleScans) #self.bridge = CvBridge() #self.zarj_os.zps.look_down() while True: begin = rospy.get_rostime() rospy.sleep(3.0) resp = self.scan_service(begin, rospy.get_rostime()) self.laser_publisher.publish(resp.cloud) #print "Got cloud {}x{}".format(resp.cloud.height, resp.cloud.width) img = self.create_image_from_cloud(resp.cloud.points) cv2.destroyAllWindows() print "New image" cv2.imshow("My image", img) cv2.waitKey(1) #print resp #cv_image = self.bridge.imgmsg_to_cv2(resp.cloud, "bgr8") #cv2.imshow("Point cloud", cv_image)
def run(self): rospy.loginfo('start task {} checkpoint {}.'.format(self.task.task_id, self.checkpoint_id)) self.start_time = rospy.get_rostime() self.setup() while not self.stopped or not self.is_done(): self.periodic() rospy.sleep(0.01) self.cleanup() rospy.loginfo('end task {} checkpoint {}.'.format(self.task.task_id, self.checkpoint_id))
def publish(ac): pub = rospy.Publisher("/aide/temperature", AirConditionerMessage, queue_size=42) rate = rospy.Rate(2) while not rospy.is_shutdown(): pub.publish(AirConditionerMessage(ac.temperature, "simulatedRoom0", rospy.get_rostime())) rate.sleep()
def callback3(self, msg): now = rospy.get_rostime() self.time3.append(now.secs) self.values.append(msg.data)
def handle_sensor(self, msg): # TODO maybe time stamp sensor values with header # TODO rospy.get_rostime() vs rospy.Time.now()? # print(rospy.get_rostime(), rospy.Time.now()) # They're different # self.sensor_t.append(rospy.Time()) self.sensor_values.append(msg.data) #print (self.sensor_values)
def relax_arm(self, arm): """ Relax one of the arms of the robot. Args: arm: Either TRIAL_ARM or AUXILIARY_ARM. """ relax_command = RelaxCommand() relax_command.id = self._get_next_seq_id() relax_command.stamp = rospy.get_rostime() relax_command.arm = arm self._relax_service.publish_and_wait(relax_command)
def update(self): with self._mutex: diag = DiagnosticArray() diag.header.stamp = rospy.get_rostime() info_update_ok = rospy.get_time() - self._last_info_update < 5.0 / self._batt_info_rate state_update_ok = rospy.get_time() - self._last_state_update < 5.0 / self._batt_state_rate if info_update_ok: self._msg.design_capacity = self._batt_design_capacity self._msg.capacity = self._batt_last_full_capacity else: self._msg.design_capacity = 0.0 self._msg.capacity = 0.0 if info_update_ok and state_update_ok and self._msg.capacity != 0: self._msg.percentage = int(self._msg.charge / self._msg.capacity * 100.0) diag_stat = _laptop_charge_to_diag(self._msg) if not info_update_ok or not state_update_ok: diag_stat.level = DiagnosticStatus.ERROR diag_stat.message = 'Laptop battery data stale' diag.status.append(diag_stat) self._diag_pub.publish(diag) self._power_pub.publish(self._msg)
def ekf_pub(self, ranges, vel_data, yaw, alt): z = np.array([]) new_pose = Odometry() ps_cov = PoseWithCovarianceStamped() for tag_name in self.tag_order: measurement = ranges[tag_name] z = np.append(z, measurement) if self.last_time is None: self.last_time = rospy.Time.now().to_sec() else: dt = rospy.Time.now().to_sec() - self.last_time self.predict(dt) self.update(z, vel_data, yaw, alt) self.last_time = rospy.Time.now().to_sec() new_pose.header.stamp = rospy.get_rostime() new_pose.header.frame_id = self.frame_id new_pose.pose.pose.position.x = self.x[0] new_pose.pose.pose.position.y = self.x[1] new_pose.pose.pose.position.z = self.x[2] cov = self.P.flatten().tolist() new_pose.pose.covariance = cov new_pose.twist.twist.linear.x = self.x[3] new_pose.twist.twist.linear.y = self.x[4] new_pose.twist.twist.linear.z = self.x[5] return new_pose # @n.publisher(EKF_COV_TOPIC, PoseWithCovarianceStamped) # def cov_pub(self, ps_cov): # return ps_cov
def add(self, msg, my_queue, my_queue_index=None): """Adds a message to the current queue, and matches them accordingly. Args: msg: Message. my_queue: Current message queue map from ROS Time to ROS message. my_queue_index: Unused. """ # Store when this message was received. received = rospy.get_rostime() # Acquire lock. self.lock.acquire() # Add to queue. my_queue[received] = msg while len(my_queue) > self.queue_size: del my_queue[min(my_queue)] # Signal by approximate time, as per ros_comm source code. # Credits to Willow Garage, Inc. # TODO: Clean this up. for vv in itertools.product(*[list(q.keys()) for q in self.queues]): qt = zip(self.queues, vv) if (((max(vv) - min(vv)) < self.slop) and (len([1 for q, t in qt if t not in q]) == 0)): msgs = [q[t] for q, t in qt] self.signalMessage(*msgs) for q, t in qt: del q[t] # Unlock. self.lock.release()
def __get_flyzone(cls, data, frame): """ Deserializes flight boundary data into a FlyZoneArray message. Args: data: List of dictionaries. frame: Frame id for the boundaries. Returns: A FlyzoneArray message type which contains an array of FlyZone messages, which contains a polygon for the boundary, a max altitude and a min altitude. """ # Generate header for all zones. header = Header() header.stamp = rospy.get_rostime() header.frame_id = frame flyzones = FlyZoneArray() for zone in data: flyzone = FlyZone() flyzone.zone.header = header flyzone.max_alt = feet_to_meters(zone["altitude_msl_max"]) flyzone.min_alt = feet_to_meters(zone["altitude_msl_min"]) # Change boundary points to ros message of type polygon. for waypoint in zone["boundary_pts"]: point = GeoPoint() point.latitude = waypoint["latitude"] point.longitude = waypoint["longitude"] flyzone.zone.polygon.points.append(point) flyzones.flyzones.append(flyzone) return flyzones
def __get_waypoints(cls, data, frame): """ Deserializes a list of waypoints into a marker message. Args: data: List of dictionaries corresponding to waypoints. frame: Frame of the markers. Returns: A marker message of type Points, with a list of points in order corresponding to each waypoint. """ header = Header() header.stamp = rospy.get_rostime() header.frame_id = frame waypoints = WayPoints() waypoints.header = header # Ensure there is no rotation by setting w to 1. for point in data: waypoint = GeoPoint() altitude = feet_to_meters(point["altitude_msl"]) waypoint.latitude = point["latitude"] waypoint.longitude = point["longitude"] waypoint.altitude = altitude waypoints.waypoints.append(waypoint) return waypoints
def __get_search_grid(cls, data, frame): """ Deserializes a the search grid into a polygon message. Args: data: List of dictionaries corresponding to the search grid points. frame: Frame for the polygon. Returns: Message of type PolygonStamped with the bounds of the search grid. """ header = Header() header.stamp = rospy.get_rostime() header.frame_id = frame search_grid = GeoPolygonStamped() search_grid.header = header for point in data: boundary_pnt = GeoPoint() altitude = feet_to_meters(point["altitude_msl"]) boundary_pnt.latitude = point["latitude"] boundary_pnt.longitude = point["longitude"] boundary_pnt.altitude = altitude search_grid.polygon.points.append(boundary_pnt) return search_grid
def publishCmdVel(cmdvel, velPublisher, velPublisherStamped): velPublisher.publish(cmdvel) baseVelocity = TwistStamped() baseVelocity.twist = cmdvel now = rospy.get_rostime() baseVelocity.header.stamp.secs = now.secs baseVelocity.header.stamp.nsecs = now.nsecs velPublisherStamped.publish(baseVelocity)
def callback(self, cmdVelocity): self.baseVelocity.twist = cmdVelocity self.flag = 1 if self.flag: # Publish Updated TwistStamped baseVelocity = TwistStamped() baseVelocity.twist = cmdVelocity now = rospy.get_rostime() baseVelocity.header.stamp.secs = now.secs baseVelocity.header.stamp.nsecs = now.nsecs self.baseVelocityPub.publish(baseVelocity)
def drive(self): start_time =rospy.get_rostime() while not rospy.is_shutdown(): curent = rospy.get_rostime() if (curent - start_time) < rospy.Duration(1,0): self.controller.SendTakeoff() self.status =0 elif (curent - start_time) < rospy.Duration(22,0): self.controller.SetCommand(pitch=1) self.status =1 elif (curent - start_time) < rospy.Duration(23,0): self.controller.SetCommand(pitch=0) self.status =2 elif (curent - start_time) < rospy.Duration(24,0): #self.controller.SetCommand(yaw_velocity=2) self.turnLeft() self.status =3 rate =rospy.Rate(1) elif (curent - start_time) < rospy.Duration(40.5,0): self.controller.SetCommand(pitch=1) self.status =4 else: self.controller.SetCommand(pitch=0) self.controller.SendLand()
def get_current_time(self): return rospy.get_rostime()
def __init__(self): self.lock = threading.Lock() rospy.init_node("roboclaw_node",log_level=rospy.DEBUG) rospy.on_shutdown(self.shutdown) rospy.loginfo("Connecting to roboclaw") self.wheels_speeds_pub = rospy.Publisher('/motors/commanded_speeds', Wheels_speeds, queue_size=1) self.motors_currents_pub = rospy.Publisher('/motors/read_currents', Motors_currents, queue_size=1) self.address = int(rospy.get_param("~address", "128")) if self.address > 0x87 or self.address < 0x80: rospy.logfatal("Address out of range") rospy.signal_shutdown("Address out of range") # TODO need someway to check if address is correct self.MAX_ABS_LINEAR_SPEED = float(rospy.get_param("~max_abs_linear_speed", "1.0")) self.MAX_ABS_ANGULAR_SPEED = float(rospy.get_param("~max_abs_angular_speed", "1.0")) self.TICKS_PER_METER = float(rospy.get_param("~ticks_per_meter", "4342.2")) self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315")) self.ACC_LIM = float(rospy.get_param("~acc_lim", "0.1")) self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH) self.last_set_speed_time = rospy.get_rostime() rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback, queue_size=1) rospy.sleep(1) rospy.logdebug("address %d", self.address) rospy.logdebug("max_abs_linear_speed %f", self.MAX_ABS_LINEAR_SPEED) rospy.logdebug("max_abs_angular_speed %f", self.MAX_ABS_ANGULAR_SPEED) rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER) rospy.logdebug("base_width %f", self.BASE_WIDTH)
def cmd_vel_callback(self, twist): with self.lock: self.last_set_speed_time = rospy.get_rostime() linear_x = twist.linear.x angular_z = twist.angular.z if abs(linear_x) > self.MAX_ABS_LINEAR_SPEED: linear_x = copysign(self.MAX_ABS_LINEAR_SPEED, linear_x) if abs(angular_z) > self.MAX_ABS_ANGULAR_SPEED: angular_z = copysign(self.MAX_ABS_ANGULAR_SPEED, angular_z) vr = linear_x - angular_z * self.BASE_WIDTH / 2.0 # m/s vl = linear_x + angular_z * self.BASE_WIDTH / 2.0 vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s vl_ticks = int(vl * self.TICKS_PER_METER) v_wheels= Wheels_speeds() v_wheels.wheel1=vl v_wheels.wheel2=vr self.wheels_speeds_pub.publish(v_wheels) rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks) # TODO: Need to make this work when more than one error is raised
def cmd_vel_callback(self, twist): with self.lock: self.last_set_speed_time = rospy.get_rostime() linear_x = twist.linear.x angular_z = twist.angular.z if abs(linear_x) > self.MAX_ABS_LINEAR_SPEED: linear_x = copysign(self.MAX_ABS_LINEAR_SPEED, linear_x) if abs(angular_z) > self.MAX_ABS_ANGULAR_SPEED: angular_z = copysign(self.MAX_ABS_ANGULAR_SPEED, angular_z) vr = linear_x - angular_z * self.BASE_WIDTH / 2.0 # m/s vl = linear_x + angular_z * self.BASE_WIDTH / 2.0 vr_ticks = int(vr * self.TICKS_PER_METER) # ticks/s vl_ticks = int(vl * self.TICKS_PER_METER) v_wheels= Wheels_speeds() v_wheels.wheel1=vl v_wheels.wheel2=vr self.wheels_speeds_pub.publish(v_wheels) rospy.logdebug("vr_ticks:%d vl_ticks: %d", vr_ticks, vl_ticks) try: #Replaced to implement watchdog #roboclaw.SpeedM1M2(self.address, vr_ticks, vl_ticks) #Replaced to implement acc #roboclaw.SpeedDistanceM1M2(self.address, vr_ticks, int(abs(vr_ticks*0.04)), vl_ticks, int(abs(vl_ticks*0.04)), 1) #rospy.logdebug(" Acc ticks %d" % (int(self.ACC_LIM * self.TICKS_PER_METER))) roboclaw.SpeedAccelDistanceM1(self.address, int(self.ACC_LIM * self.TICKS_PER_METER),vr_ticks, int(abs(vr_ticks*0.04)),1) roboclaw.SpeedAccelDistanceM2(self.address, int(self.ACC_LIM * self.TICKS_PER_METER),vl_ticks, int(abs(vl_ticks*0.04)),1) #Mixed command doesn't work #roboclaw.SpeedAccelDistanceM1M2(self.address, int(self.ACC_LIM * self.TICKS_PER_METER),vr_ticks, int(abs(vr_ticks*0.04)), vl_ticks, int(abs(vl_ticks*0.04)), 1) except OSError as e: rospy.logwarn("SpeedM1M2 OSError: %d", e.errno) rospy.logdebug(e) # TODO: Need to make this work when more than one error is raised
def detection(self, hsv_image): """Check for detection in the image """ mask = cv2.inRange(hsv_image, self.color_low, self.color_high) if self.baseline_cnt > 0: nmask = cv2.bitwise_not(mask) if self.baseline is None: rospy.loginfo("getting baseline for {}".format(self.name)) self.baseline = nmask else: self.baseline = cv2.bitwise_or(nmask, self.baseline) mask = cv2.bitwise_and(mask, self.baseline) count = cv2.countNonZero(mask) + self.baseline_fuzzy self.low_count = max(self.low_count, count) self.high_count = self.low_count + self.baseline_fuzzy self.baseline_cnt -= 1 return elif self.baseline is not None: mask = cv2.bitwise_and(mask, self.baseline) count = cv2.countNonZero(mask) if count > self.low_count and self.active is None: self.active = rospy.get_rostime() rospy.loginfo("{} ACTIVE ({})".format(self.name, count)) self.cloud.reset() if self.callbacks[0] is not None: self.callbacks[0](self.name) elif self.active is not None and count < self.high_count: rospy.loginfo("{} GONE ({})".format(self.name, count)) self.cloud.reset() self.active = None if self.callbacks[2] is not None: self.published = False self.report_count = 0 if self.callbacks[1] is not None: self.callbacks[1](self.name) # DEBUGGING to see what the masked image for the request is if self.debug: cv2.namedWindow(self.name, cv2.WINDOW_NORMAL) if self.baseline is not None: cv2.namedWindow(self.name+'_baseline', cv2.WINDOW_NORMAL) cv2.imshow(self.name+'_baseline', self.baseline) cv2.imshow(self.name, mask) cv2.waitKey(1) # to to see if we notify the location callback if self.is_active() and self.report_count > self.min_reports: now = rospy.get_rostime() if (self.active + self.stablize_time) < now: self.published = True point = PointStamped() center = self.cloud.find_center() point.header.seq = 1 point.header.stamp = now point.header.frame_id = self.frame_id point.point.x = center[0] point.point.y = center[1] point.point.z = center[2] if self.callbacks[2] is not None: self.callbacks[2](self.name, point)
def publish_tf(self): model_cache = {} poses = {'gazebo_world': identity_matrix()} for (link_idx, link_name) in enumerate(self.link_states_msg.name): poses[link_name] = pysdf.pose_msg2homogeneous(self.link_states_msg.pose[link_idx]) # print('%s:\n%s' % (link_name, poses[link_name])) for (link_idx, link_name) in enumerate(self.link_states_msg.name): # print(link_idx, link_name) modelinstance_name = link_name.split('::')[0] # print('modelinstance_name:', modelinstance_name) model_name = pysdf.name2modelname(modelinstance_name) # print('model_name:', model_name) if not model_name in model_cache: sdf = pysdf.SDF(model=model_name) model_cache[model_name] = sdf.world.models[0] if len(sdf.world.models) >= 1 else None if not model_cache[model_name]: print('Unable to load model: %s' % model_name) model = model_cache[model_name] link_name_in_model = link_name.replace(modelinstance_name + '::', '') if model: link = model.get_link(link_name_in_model) if link.tree_parent_joint: parent_link = link.tree_parent_joint.tree_parent_link parent_link_name = parent_link.get_full_name() # print('parent:', parent_link_name) parentinstance_link_name = parent_link_name.replace(model_name, modelinstance_name, 1) else: # direct child of world parentinstance_link_name = 'gazebo_world' else: # Not an SDF model parentinstance_link_name = 'gazebo_world' # print('parentinstance:', parentinstance_link_name) pose = poses[link_name] #parent_pose = pysdf.pose_msg2homogeneous(self.model_states_msg.pose[1]) parent_pose = poses[parentinstance_link_name] rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose) translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf) # print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion)) self.tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name)) # Main function.
def run(self): rospy.loginfo("Starting motor drive") r_time = rospy.Rate(30) while not rospy.is_shutdown(): with self.lock: if (rospy.get_rostime() - self.last_set_speed_time).to_sec() > 1: rospy.loginfo("Did not get comand for 1 second, stopping") try: roboclaw.ForwardM1(self.address, 0) roboclaw.ForwardM2(self.address, 0) except OSError as e: rospy.logerr("Could not stop") rospy.logdebug(e) # TODO need find solution to the OSError11 looks like sync problem with serial status1, enc1, crc1 = None, None, None status2, enc2, crc2 = None, None, None statusC, amp1, amp2 = None, None, None try: status1, enc1, crc1 = roboclaw.ReadEncM1(self.address) except ValueError: pass try: status2, enc2, crc2 = roboclaw.ReadEncM2(self.address) except ValueError: pass try: status1c, amp1, amp2 = roboclaw.ReadCurrents(self.address) except ValueError: pass if (enc1 != None) & (enc2 != None): rospy.logdebug(" Encoders %d %d" % (enc1, enc2)) self.encodm.update_publish(enc1, enc2) self.updater.update() else: rospy.logdebug("Error Reading enc") if (amp1 != None) & (amp2 != None): rospy.logdebug(" Currents %d %d" % (amp1, amp2)) amps=Motors_currents() amps.motor1=float(amp1)/100 amps.motor2=float(amp2)/100 self.motors_currents_pub.publish(amps) else: rospy.logdebug("Error Reading Currents") r_time.sleep()
