def to_path_marker(self, node: Node, clear=False): """Return a `nav_msgs/Path` message with all waypoints in the set > *Input arguments* * `clear` (*type:* `bool`, *default:* `False`): Return an empty `nav_msgs/Path` message. > *Returns* `nav_msgs/Path` message """ path = Path() t = node.get_clock().now() path.header.stamp = t.to_msg() path.header.frame_id = self._inertial_frame_id if self.num_waypoints > 1 and not clear: for i in range(self.num_waypoints): wp = self.get_waypoint(i) pose = PoseStamped() pose.header.stamp = rclpy.time.Time(seconds=i) # TODO check pose.header.frame_id = self._inertial_frame_id pose.pose.position.x = wp.x pose.pose.position.y = wp.y pose.pose.position.z = wp.z path.poses.append(pose) return path
def timer_callback(self): """Timer Callback Function This method captures images and publishes required data in ros 2 topic. """ if self.capture.isOpened(): # reads image data ret, frame = self.capture.read() # processes image data and converts to ros 2 message msg = Image() msg.header.stamp = Node.get_clock(self).now().to_msg() msg.header.frame_id = 'ANI717' msg.height = np.shape(frame)[0] msg.width = np.shape(frame)[1] msg.encoding = "bgr8" msg.is_bigendian = False msg.step = np.shape(frame)[2] * np.shape(frame)[1] msg.data = np.array(frame).tobytes() # publishes message self.publisher_.publish(msg) self.get_logger().info('%d Images Published' % self.i) # image counter increment self.i += 1 return None
def run(self, node: Node, joy_state: sensor_msgs.msg.Joy) -> None: # The logic for responding to this joystick press is: # 1. Save off the current state of active. # 2. Update the current state of active based on buttons and axes. # 3. If this command is currently not active, return without publishing. # 4. If this is a msg_value, and the value of the previous active is the same as now, # debounce and return without publishing. # 5. In all other cases, publish. This means that this is a msg_value and the button # transitioned from 0 -> 1, or it means that this is an axis mapping and data should # continue to be published without debouncing. last_active = self.active self.update_active_from_buttons_and_axes(joy_state) if not self.active: return if self.msg_value is not None and last_active == self.active: return if self.msg_value is not None: # This is the case for a static message. msg = self.msg_value else: # This is the case to forward along mappings. msg = self.topic_type() for mapping, values in self.axis_mappings.items(): if 'axis' in values: if len(joy_state.axes) > values['axis']: val = joy_state.axes[values['axis']] * values.get('scale', 1.0) + \ values.get('offset', 0.0) else: node.get_logger().error('Joystick has only {} axes (indexed from 0),' 'but #{} was referenced in config.'.format( len(joy_state.axes), values['axis'])) val = 0.0 elif 'button' in values: if len(joy_state.buttons) > values['button']: val = joy_state.buttons[values['button']] * values.get('scale', 1.0) + \ values.get('offset', 0.0) else: node.get_logger().error('Joystick has only {} buttons (indexed from 0),' 'but #{} was referenced in config.'.format( len(joy_state.buttons), values['button'])) val = 0.0 elif 'value' in values: # Pass on the value as its Python-implicit type val = values.get('value') else: node.get_logger().error( 'No Supported axis_mappings type found in: {}'.format(mapping)) val = 0.0 set_member(msg, mapping, val) # If there is a stamp field, fill it with now(). if hasattr(msg, 'header'): msg.header.stamp = node.get_clock().now().to_msg() self.pub.publish(msg)
def __init__(self,origion_x=0.0,origion_y=0.0,resolution=0.1, width=50, height=50): super().__init__('map_pub') self.msg = OccupancyGrid() #self.pub = self.create_publisher(OccupancyGrid) #timer_period = 0.05 #self.timer = self.create_timer(timer_period, self.scan) self.time = Node.get_clock(self) self.time = self.get_clock().now().to_msg() self.orogion_x = origion_x self.origion_y = origion_y self.resolution = resolution self.width = width self.height = height self.grid = np.zeros((height, width)) self.time = Node.get_clock(self) self.time = self.get_clock().now().to_msg()
def __init__(self): super().__init__('pose_estimate') self.msg = geometry_msgs.msg.TransformStamped() self.pub = self.create_publisher(geometry_msgs.msg.TransformStamped, "/tf", 1) timer_period = 0.05 self.timer = self.create_timer(timer_period, self.scan) self.time = Node.get_clock(self) self.time = self.get_clock().now().to_msg()
def __init__(self): _node = Node("tf2twist") qos_profile = QoSProfile(depth=10) # loop_rate = _node.create_rate(100) tfBuffer = Buffer() listener = TransformListener(tfBuffer, _node, qos=qos_profile) twist_data = geometry_msgs.msg.Twist() pub_cmd_vel = _node.create_publisher(geometry_msgs.msg.Twist, "cmd_vel", qos_profile) # self.transfromstamped = try: while rclpy.ok(): rclpy.spin_once(_node) now = _node.get_clock().now() - rclpy.duration.Duration( seconds=0, nanoseconds=1000000) try: trans = tfBuffer.lookup_transform( 'odom_frame', 'base_link', now, rclpy.duration.Duration(seconds=0, nanoseconds=0)) # print(trans) # print(trans.transform.rotation.x) roll, pitch, yaw = self.quaternion_to_euler( trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) # print(yaw) if (yaw > 0.3): twist_data.angular.z = 70.0 #yaw*125*1.3 elif (yaw < -0.3): twist_data.angular.z = -70.0 #yaw*125*1.3 else: twist_data.angular.z = 0.0 pub_cmd_vel.publish(twist_data) except (LookupException, LookupError, ConnectionAbortedError, ConnectionError, ConnectionRefusedError, ConnectionResetError, ExtrapolationException, ConnectivityException) as e: # print(e) pass except (KeyboardInterrupt): pass
class HardwareControlManager: def __init__(self): rclpy.init(args=None) self.node = Node("hcm", allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) multi_executor = MultiThreadedExecutor() multi_executor.add_node(self.node) self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True) #self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) self.spin_thread.start() self.blackboard = None # --- Initialize Node --- # Otherwise messages will get lost, bc the init is not finished self.node.get_clock().sleep_for(Duration(seconds=0.1)) self.node.get_logger().debug("Starting hcm") self.simulation_active = self.node.get_parameter("simulation_active") # dsd self.blackboard = HcmBlackboard(self.node) self.blackboard.animation_action_client = ActionClient( self.node, PlayAnimation, 'animation') self.blackboard.dynup_action_client = ActionClient( self.node, Dynup, 'dynup') dirname = os.path.dirname(os.path.realpath(__file__)) + "/hcm_dsd" self.dsd = DSD(self.blackboard, "debug/dsd/hcm", node=self.node) self.dsd.register_actions(os.path.join(dirname, 'actions')) self.dsd.register_decisions(os.path.join(dirname, 'decisions')) self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd')) self.hcm_deactivated = False # Publisher / subscriber self.joint_goal_publisher = self.node.create_publisher( JointCommand, 'DynamixelController/command', 1) self.hcm_state_publisher = self.node.create_publisher( RobotControlState, 'robot_state', 1) # todo latch self.blackboard.speak_publisher = self.node.create_publisher( Audio, 'speak', 1) # important to make sure the connection to the speaker is established, for next line self.node.get_clock().sleep_for(Duration(seconds=0.1)) speak("Starting hcm", self.blackboard.speak_publisher, priority=50) self.node.create_subscription(Imu, "imu/data", self.update_imu, 1) self.node.create_subscription(FootPressure, "foot_pressure_left/filtered", self.update_pressure_left, 1) self.node.create_subscription(FootPressure, "foot_pressure_right/filtered", self.update_pressure_right, 1) self.node.create_subscription(JointCommand, "walking_motor_goals", self.walking_goal_callback, 1) self.node.create_subscription(AnimationMsg, "animation", self.animation_callback, 1) self.node.create_subscription(JointCommand, "dynup_motor_goals", self.dynup_callback, 1) self.node.create_subscription(JointCommand, "head_motor_goals", self.head_goal_callback, 1) self.node.create_subscription(JointCommand, "record_motor_goals", self.record_goal_callback, 1) self.node.create_subscription(JointCommand, "kick_motor_goals", self.kick_goal_callback, 1) self.node.create_subscription(Bool, "pause", self.pause, 1) self.node.create_subscription(JointState, "joint_states", self.joint_state_callback, 1) self.node.create_subscription(PointStamped, "cop_l", self.cop_l_cb, 1) self.node.create_subscription(PointStamped, "cop_r", self.cop_r_cb, 1) self.node.create_subscription(Bool, "core/power_switch_status", self.power_cb, 1) self.node.create_subscription(Bool, "hcm_deactivate", self.deactivate_cb, 1) self.node.create_subscription(DiagnosticArray, "diagnostics_agg", self.blackboard.diag_cb, 1) self.node.add_on_set_parameters_callback(self.on_set_parameters) self.main_loop() def deactivate_cb(self, msg): self.hcm_deactivated = msg.data def pause(self, msg): """ Updates the stop state for the state machine""" self.blackboard.stopped = msg.data def update_imu(self, msg): """Gets new IMU values and computes the smoothed values of these""" self.blackboard.last_imu_update_time = msg.header.stamp self.blackboard.accel = numpy.array([ msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z ]) self.blackboard.gyro = numpy.array([ msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z ]) self.blackboard.quaternion = numpy.array(([ msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w ])) self.blackboard.smooth_gyro = numpy.multiply( self.blackboard.smooth_gyro, 0.95) + numpy.multiply( self.blackboard.gyro, 0.05) self.blackboard.smooth_accel = numpy.multiply( self.blackboard.smooth_accel, 0.99) + numpy.multiply( self.blackboard.accel, 0.01) self.blackboard.not_much_smoothed_gyro = numpy.multiply( self.blackboard.not_much_smoothed_gyro, 0.5) + numpy.multiply( self.blackboard.gyro, 0.5) self.blackboard.imu_msg = msg def update_pressure_left(self, msg): """Gets new pressure values and writes them to the blackboard""" self.blackboard.last_pressure_update_time = msg.header.stamp self.blackboard.pressures[0] = msg.left_front self.blackboard.pressures[1] = msg.left_back self.blackboard.pressures[2] = msg.right_front self.blackboard.pressures[3] = msg.right_back def update_pressure_right(self, msg): """Gets new pressure values and writes them to the blackboard""" self.blackboard.last_pressure_update_time = msg.header.stamp self.blackboard.pressures[4] = msg.left_front self.blackboard.pressures[5] = msg.left_back self.blackboard.pressures[6] = msg.right_front self.blackboard.pressures[7] = msg.right_back def on_set_parameters(self, config, level): """ Dynamic reconfigure of the fall checker values.""" # just pass on to the StandupHandler, as all the variables are located there self.blackboard.fall_checker.update_reconfigurable_values( config, level) self.blackboard.pickup_accel_threshold = config[ "pick_up_accel_threshold"] return config def walking_goal_callback(self, msg): self.blackboard.last_walking_goal_time = self.node.get_clock().now() if self.blackboard.current_state in [ RobotControlState.CONTROLLABLE, RobotControlState.WALKING ]: self.joint_goal_publisher.publish(msg) def dynup_callback(self, msg): if self.blackboard.current_state in [ RobotControlState.STARTUP, RobotControlState.FALLEN, RobotControlState.GETTING_UP, RobotControlState.CONTROLLABLE ]: self.joint_goal_publisher.publish(msg) def head_goal_callback(self, msg): if self.blackboard.current_state in [ RobotControlState.CONTROLLABLE, RobotControlState.WALKING ]: # we can move our head self.joint_goal_publisher.publish(msg) def record_goal_callback(self, msg): if msg.joint_names == []: # record tells us that its finished self.blackboard.record_active = False else: self.blackboard.record_active = True self.joint_goal_publisher.publish(msg) def kick_goal_callback(self, msg): if self.blackboard.current_state in [ RobotControlState.KICKING, RobotControlState.CONTROLLABLE ]: # we can perform a kick self.joint_goal_publisher.publish(msg) def animation_callback(self, msg): """ The animation server is sending us goal positions for the next keyframe""" self.blackboard.last_animation_goal_time = msg.header.stamp if msg.request: self.node.get_logger().info( "Got Animation request. HCM will try to get controllable now.") # animation has to wait # dsd should try to become controllable self.blackboard.animation_requested = True return if msg.first: if msg.hcm: # coming from ourselves # we don't have to do anything, since we must be in the right state pass else: # coming from outside # check if we can run an animation now if self.blackboard.current_state != RobotControlState.CONTROLLABLE: self.node.get_logger().warn( "HCM is not controllable, animation refused.") return else: # we're already controllable, go to animation running self.blackboard.external_animation_running = True if msg.last: if msg.hcm: # This was an animation from the DSD self.blackboard.hcm_animation_finished = True pass else: # this is the last frame, we want to tell the DSD that we're finished with the animations self.blackboard.external_animation_running = False if msg.position is None: # probably this was just to tell us we're finished # we don't need to set another position to the motors return # forward positions to motors, if some where transmitted if len( msg.position.points ) > 0 and self.blackboard.current_state != RobotControlState.GETTING_UP: out_msg = JointCommand() out_msg.positions = msg.position.points[0].positions out_msg.joint_names = msg.position.joint_names out_msg.accelerations = [-1.0] * len(out_msg.joint_names) out_msg.velocities = [-1.0] * len(out_msg.joint_names) out_msg.max_currents = [-1.0] * len(out_msg.joint_names) if msg.position.points[0].effort: out_msg.max_currents = [ -x for x in msg.position.points[0].effort ] if self.blackboard.shut_down_request: # there are sometimes transmission errors during shutdown due to race conditions # there is nothing we can do so just ignore the errors in this case try: self.joint_goal_publisher.publish(out_msg) except: pass else: self.joint_goal_publisher.publish(out_msg) def joint_state_callback(self, msg: JointState): self.blackboard.last_motor_update_time = msg.header.stamp self.blackboard.previous_joint_state = self.blackboard.current_joint_state self.blackboard.current_joint_state = msg def cop_l_cb(self, msg): self.blackboard.cop_l_msg = msg def cop_r_cb(self, msg): self.blackboard.cop_r_msg = msg def power_cb(self, msg): self.blackboard.is_power_on = msg.data def main_loop(self): """ Keeps updating the DSD and publish its current state. All the forwarding of joint goals is directly done in the callbacks to reduce latency. """ last_loop_start_time = self.node.get_clock().now() while rclpy.ok() and not self.blackboard.shut_down_request: try: loop_start_time = self.node.get_clock().now() #can happen in simulation due to bad implementation in rclpy if (last_loop_start_time != loop_start_time): last_loop_start_time = loop_start_time if self.hcm_deactivated: self.blackboard.current_state = RobotControlState.CONTROLLABLE msg = RobotControlState() msg.state = self.blackboard.current_state self.hcm_state_publisher.publish(msg) else: self.blackboard.current_time = self.node.get_clock( ).now() try: self.dsd.update() msg = RobotControlState() msg.state = self.blackboard.current_state self.hcm_state_publisher.publish(msg) except IndexError: # this error will happen during shutdown procedure, just ignore it pass self.node.get_clock().sleep_until(loop_start_time + Duration(seconds=1 / 500.0)) except (ExternalShutdownException, KeyboardInterrupt): if not self.simulation_active: self.on_shutdown_hook() exit(0) if not self.simulation_active: self.on_shutdown_hook() self.spin_thread.join() def on_shutdown_hook(self): if not self.blackboard: return # we got external shutdown, tell it to the DSD, it will handle it self.blackboard.shut_down_request = True self.node.get_logger().warn( "You're stopping the HCM. The robot will sit down and power off its motors." ) speak("Stopping HCM", self.blackboard.speak_publisher, priority=50) # now wait for it finishing the shutdown procedure while not self.blackboard.current_state == RobotControlState.HCM_OFF: # we still have to update everything self.blackboard.current_time = self.node.get_clock().now() self.dsd.update() self.hcm_state_publisher.publish(self.blackboard.current_state) self.node.get_clock().sleep_for(Duration(seconds=0.01))
def main(): rclpy.init(args=None) node = Node('system_monitor') pub = node.create_publisher(WorkloadMsg, 'system_workload', 1) diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1) hostname = socket.gethostname() diag_array = DiagnosticArray() diag_cpu = DiagnosticStatus() # start all names with "SYSTEM" for diagnostic analysesr diag_cpu.name = "SYSTEMCPU" diag_cpu.hardware_id = "CPU" diag_mem = DiagnosticStatus() diag_mem.name = "SYSTEMMemory" diag_cpu.hardware_id = "Memory" node.declare_parameter('update_frequency', 10.0) node.declare_parameter('do_memory', True) node.declare_parameter('do_cpu', True) node.declare_parameter('cpu_load_percentage', 80.0) node.declare_parameter('memoroy_load_percentage', 80.0) node.declare_parameter('network_rate_received_errors', 10.0) node.declare_parameter('network_rate_send_errors', 10.0) rate = node.get_parameter('update_frequency').get_parameter_value().double_value do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value network_rate_received_errors = node.get_parameter( 'network_rate_received_errors').get_parameter_value().double_value network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value while rclpy.ok(): last_send_time = time.time() running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else ( -1, [], 0) memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1) interfaces = network_interfaces.collect_all(node.get_clock()) msg = WorkloadMsg( hostname=hostname, cpus=cpu_usages, running_processes=running_processes, cpu_usage_overall=overall_usage_percentage, memory_available=memory_available, memory_used=memory_used, memory_total=memory_total, filesystems=[], network_interfaces=interfaces ) pub.publish(msg) diag_array.status = [] # publish diagnostic message to show this in the rqt diagnostic viewer diag_cpu.message = str(overall_usage_percentage) + "%" if overall_usage_percentage >= cpu_load_percentage: diag_cpu.level = DiagnosticStatus.WARN else: diag_cpu.level = DiagnosticStatus.OK diag_array.status.append(diag_cpu) memory_usage = round((memory_used / memory_total) * 100, 2) diag_mem.message = str(memory_usage) + "%" if memory_usage >= memoroy_load_percentage: diag_mem.level = DiagnosticStatus.WARN else: diag_mem.level = DiagnosticStatus.OK diag_array.status.append(diag_mem) for interface in interfaces: diag_net = DiagnosticStatus() diag_net.name = "SYSTEM" + interface.name diag_net.hardware_id = interface.name if interface.rate_received_packets_errors >= network_rate_received_errors \ or interface.rate_sent_packets_errors >= network_rate_send_errors: diag_net.level = DiagnosticStatus.WARN else: diag_net.level = DiagnosticStatus.OK diag_array.status.append(diag_net) diag_array.header.stamp = node.get_clock().now().to_msg() diagnostic_pub.publish(diag_array) # sleep to have correct rate. we dont use ROS time since we are interested in system things dt = time.time() - last_send_time time.sleep(max(0, (1 / rate) - dt))
red_request.leds = orange_leds_array else: red_request.leds = red_leds_array previous_req.leds = led_serv.call(red_request).previous_leds def reset_leds(): global leds_red, previous_req leds_red = False led_serv.call(previous_req) # wait a moment on startup, otherwise we will think there is a problem while ros control is still booting set_leds_service = node.create_client(Leds, "/set_leds") set_leds_service.wait_for_service() node.get_clock().sleep_for(Duration(seconds=1)) node.create_subscription(DiagnosticStatus, "/diagnostics_toplevel_state", cb, 1) rate = node.create_rate(100) while rclpy.ok(): if last_hardware_error_time is not None: current_time = node.get_clock().now().to_sec() if currently_blinking: # we are currently blinking, check if the last hardware error is to long ago if current_time - last_hardware_error_time > ERROR_TIMEOUT: currently_blinking = False reset_leds() led_set_time = current_time else:
pos_msg.positions = [0, 0] pos_msg.velocities = [1.5, 1.5] pos_msg.accelerations = [-1, -1] pos_msg.max_currents = [-1, -1] rclpy.wait_for_service("bio_ik/get_bio_ik") bio_ik = node.create_client(GetIK, 'bio_ik/get_bio_ik') publish_motor_goals = node.create_publisher(JointCommand, 'head_motor_goals', 10) while rclpy.ok(): x = float(input('x: ')) y = float(input('y: ')) point = PointStamped() point.header.stamp = node.get_clock().now() point.header.frame_id = base_footprint_frame point.point.x = x point.point.y = y point.point.z = 0 ###### Movement ###### point = tf_buffer.transform(point, head_tf_frame, timeout=Duration(seconds=0.3)).point target = Point(point.x, point.y, point.z) request.look_at_goals[0].target = target response = bio_ik(request).ik_response states = response.solution.joint_state head_pan = states.position[states.name.index('HeadPan')] head_tilt = states.position[states.name.index('HeadTilt')]
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from std_srvs.srv import Empty from bitbots_msgs.msg import Buttons from rclpy.node import Node from rclpy.duration import Duration rclpy.init(args=None) node = Node('zero_on_button') zero_l = node.create_client(Empty, "/foot_pressure_left/set_foot_zero") zero_r = node.create_client(Empty, "/foot_pressure_right/set_foot_zero") button_prev_state = False press_time = node.get_clock().now() - Duration(seconds=1.0) def cb(msg): global button_prev_state, press_time print("New msg") print(msg.button1) print(not button_prev_state) print(node.get_clock().now() - press_time > Duration(seconds=1.0)) if msg.button3 and not button_prev_state and node.get_clock().now( ) - press_time > Duration(seconds=1.0): zero_l() zero_r() press_time = node.get_clock() button_prev_state = msg.button3