def __init__(self): self.pcl_pub = rospy.Publisher("/rqt_pcl_visualizer/point_cloud2", PointCloud2, queue_size=5) self.buffer_size_pub = rospy.Publisher( "/rqt_pcl_visualizer/buffer_size", UInt32, queue_size=2) rospy.sleep(1.0) self.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgba', 12, PointField.UINT32, 1), ] self.header = Header() self.header.frame_id = 'map' # while not rospy.is_shutdown(): if True: dt = 0.05 rospy.loginfo("small buffer mode") self.buffer_size_pub.publish(UInt32(10)) rospy.sleep(0.4) self.pub_sphere(dt) self.pub_sphere(dt, 0.02) rospy.sleep(1.3) rospy.loginfo("unlimited buffer mode") self.buffer_size_pub.publish(UInt32(0)) rospy.sleep(0.4) self.pub_sphere(dt, 0.03) rospy.sleep(1.3) rospy.loginfo("buffer size 1 mode") self.buffer_size_pub.publish(UInt32(1)) self.pub_sphere(dt, 0.04)
def _floorplan_to_msg(floorplan): floorplan_msg = Floorplan() node_id = {} for i, u in enumerate(floorplan.nodes): node_id[u] = i scene_node_msg = SceneNode() scene_node_msg.free_ratio.data = u.free_ratio scene_node_msg.index_map_keys = [ UInt32(data=k) for k in u.vertex_index_map.keys() ] scene_node_msg.index_map_values = [ UInt32(data=v) for v in u.vertex_index_map.values() ] for vertex in u.vertices: point_msg = Point() point_msg.x = vertex[0] point_msg.y = vertex[1] point_msg.z = vertex[2] scene_node_msg.vertices.append(point_msg) for edge in u.edges: pair_msg = UInt32Pair() pair_msg.u.data = edge[0] pair_msg.v.data = edge[1] scene_node_msg.edges.append(pair_msg) floorplan_msg.nodes.append(scene_node_msg) for u, v, data in floorplan.edges(data=True, keys=False): scene_edge_msg = SceneEdge() scene_edge_msg.u.data = node_id[u] scene_edge_msg.v.data = node_id[v] shared_edge = data['shared_edge'] if shared_edge is not None: scene_edge_msg.shared_edge = [ UInt32(data=shared_edge[0]), UInt32(data=shared_edge[1]) ] if data['boundary_interval'] is not None: interval = data['boundary_interval'] start_point = Point() start_point.x = interval[0][0] start_point.y = interval[0][1] start_point.z = interval[0][2] scene_edge_msg.boundary.append(start_point) stop_point = Point() stop_point.x = interval[1][0] stop_point.y = interval[1][1] stop_point.z = interval[1][2] scene_edge_msg.boundary.append(stop_point) floorplan_msg.edges.append(scene_edge_msg) return floorplan_msg
def __init__(self): rospy.init_node("Twitter_Checker") self.response = None self.send = UInt32() self.response_pub = rospy.Publisher('/Tweet_Checker', UInt32, queue_size=1)
def __init__(self): rospy.init_node("mav_control_node") rospy.Subscriber("/mavros/global_position/global",NavSatFix,self.gps_callback) rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.pose_callback) rospy.Subscriber('mavros/state',State,self.arm_callback) rospy.Subscriber('mavros/battery',BatteryState,self.battery_callback) rospy.Subscriber('mavros/global_position/raw/satellites',UInt32,self.sat_num_callback) rospy.Subscriber('mavros/global_position/rel_alt',Float64,self.rel_alt_callback) rospy.Subscriber('mavros/imu/data',Imu,self.imu_callback) rospy.Subscriber("/mavros/rc/in", RCIn, self.rc_callback) rospy.Subscriber('mavros/vfr_hud',VFR_HUD,self.hud_callback) self.gps = NavSatFix() self.pose = PoseStamped() self.arm_status = State() self.battery_status = BatteryState() self.sat_num = UInt32() self.rel_alt = Float64() self.imu = Imu() self.rc = RCIn() self.hud = VFR_HUD() self.timestamp = rospy.Time() self.cmd_pos_pub = rospy.Publisher("/mavros/setpoint_position/local", PoseStamped, queue_size=1) self.cmd_vel_pub = rospy.Publisher("/mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=1) self.rc_override = rospy.Publisher("/mavros/rc/override", OverrideRCIn, queue_size=1) self.mode_service = rospy.ServiceProxy("/mavros/set_mode", SetMode) self.arm_service = rospy.ServiceProxy("/mavros/cmd/arming", CommandBool) self.takeoff_service = rospy.ServiceProxy("/mavros/cmd/takeoff", CommandTOL) self.stream_service = rospy.ServiceProxy("/mavros/set_stream_rate", StreamRate) self.home_service = rospy.ServiceProxy("/mavros/cmd/set_home", CommandHome)
def test_ros_message_with_uint32(self): from std_msgs.msg import UInt32 expected_dictionary = { 'data': 0xFFFFFFFF } message = UInt32(data=expected_dictionary['data']) message = serialize_deserialize(message) dictionary = message_converter.convert_ros_message_to_dictionary(message) self.assertEqual(dictionary, expected_dictionary)
def handle_user_select(req): param_control_user_selected = '/comm/param/control/target/selected' num = req.select_num selected = -1 info = "Please choose the one you want, if no target, enter 0." print info info_msg = String() info_msg.data = "CHOOSE" pubInfo.publish(info_msg) # Make sure the last select is clear if rospy.has_param(param_control_user_selected): rospy.set_param(param_control_user_selected, -1) # broadcast the request to select the target sr_msg = UInt32() sr_msg.data = num pubSR.publish(sr_msg) while selected < 0: if rospy.has_param(param_control_user_selected): selected = rospy.get_param(param_control_user_selected) if num >= selected >= 0: break else: selected = -1 rsp = user_selectResponse() rsp.selected_id = int(selected) rospy.set_param(param_control_user_selected, -1) return rsp
def publish(self): with self.lock_iteration: focus = copy(self.focus) ready = copy(self.ready_for_interaction) interests_array = self.learning.get_normalized_interests_evolution() interests = Interests() interests.names = self.learning.get_space_names() interests.num_iterations = UInt32(len(interests_array)) interests.interests = [Float32(val) for val in interests_array.flatten()] self.pub_ready.publish(Bool(data=ready)) self.pub_user_focus.publish(String(data=focus if focus is not None else "")) self.pub_interests.publish(interests) self.pub_focus.publish(String(data=self.learning.get_last_focus())) self.pub_iteration.publish(UInt32(data=self.learning.get_iterations()))
def _read(self, pathId, start, L): from math import ceil, floor N = int(ceil(abs(L) * self.frequency)) rospy.loginfo( "Start reading path {} (t in [ {}, {} ]) into {} points".format( pathId, start, start + L, N + 1)) self.reading = True self.queue = Queue.Queue(self.queue_size) times = (-1 if L < 0 else 1) * np.array(range(N + 1), dtype=float) / self.frequency times[-1] = L times += start Nv = int(ceil(float(self.frequency) / float(self.viewerFreq))) updateViewer = [False] * (N + 1) if hasattr(self, "viewer"): for i in range(0, len(updateViewer), Nv): updateViewer[i] = True updateViewer[-1] = True self.firstMsgs = None for t, uv in zip(times, updateViewer): msgs = self.readAt(pathId, t, uv, timeShift=start) if self.firstMsgs is None: self.firstMsgs = msgs self.pubs["read_path_done"].publish(UInt32(pathId)) rospy.loginfo("Finish reading path {}".format(pathId)) self.reading = False
def execute(self, userdata): """execute what needs to be executed""" msg = UInt32() msg.data = userdata.step self.pub.publish(msg) return 'done'
def __init__(self): # 声明节点订阅与发布的消息 # self.goal_name_sub = rospy.Subscriber('/office/goal_name', String, self.goal_nameCB) self.move_base_goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1) self.move_base_result_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.move_base_resultCB) self.clear_costmaps_srv = rospy.ServiceProxy( '/move_base/clear_costmaps', Empty) # 每一个讲解点的名字:位置字典 self.position_dict = dict() # 记录机器人当前的目标点 self.current_goal = "middle" self.last_goal = "origin" # 循环次数,原本是想使用逐次累加的方式计数 # 现在已改为255表示进行新的一轮循环讲解,200表示语音未识别,开启摄像头确认交互人是否还在,在的话进行继续交互 self.loop_tag = UInt32() # 读取一存储的讲解点字典文件,默认位于xbot_s/param/position_dic.yaml文件 yaml_path = rospy.get_param( '/cycle/position_dict_path', '/home/xbot/catkin_ws/src/xbot_s/param/cycle_maze.yaml') # yaml_path = yaml_path + '/scripts/position_dic.yaml' f = open(yaml_path, 'r') self.position_dict = yaml.load(f) f.close() # self.make_launch() cmd = 'rostopic pub -1 /office/next_loop std_msgs/UInt32 "data: 255" ' # os.system(cmd) rospy.spin()
def test_dictionary_with_uint32(self): from std_msgs.msg import UInt32 expected_message = UInt32(data = 0xFFFFFFFF) dictionary = { 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message('std_msgs/UInt32', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def __init__(self): self.next_loop_pub = rospy.Publisher('/office/next_loop', UInt32, queue_size=1) self.goal_reached_pub = rospy.Publisher('/office/goal_reached', String, queue_size=1) self.move_base_goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1) self.move_base_result_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.move_base_resultCB) self.goal_name_sub = rospy.Subscriber('/office/goal_name', String, self.goal_nameCB) self.clear_costmaps_srv = rospy.ServiceProxy( '/move_base/clear_costmaps', Empty) self.coll_position_dict = dict() self.current_goal = [] self.loop_tag = UInt32() self.loop_tag.data = 0 yaml_path = rospy.get_param('/office_slam/yaml_file_path', '/home/roc/catkin_ws/src/xbot_navigoals') yaml_path = yaml_path + '/scripts/coll_position_dic.yaml' f = open(yaml_path, 'r') self.coll_position_dict = yaml.load(f) f.close() rospy.spin()
def generateFlockStateMsg(self, duckies, requests, filled_requests): msg = FlockState() msg.header.stamp = rospy.Time.now() for request_id in requests: request = requests[request_id] request_msg = Request() request_msg.request_id = String(data=request.id) request_msg.start_time = UInt32(data=request.start_time) request_msg.pickup_time = UInt32(data=request.pickup_time) request_msg.start_node = String(data=request.start_node) request_msg.end_node = String(data=request.end_node) request_msg.duckie_id = String(data=request.duckie_id) msg.requests.append(request_msg) for request_id in filled_requests: request = filled_requests[request_id] request_msg = Request() request_msg.request_id = String(data=request.id) request_msg.start_time = UInt32(data=request.start_time) request_msg.pickup_time = UInt32(data=request.pickup_time) request_msg.end_time = UInt32(data=request.end_time) request_msg.start_node = String(data=request.start_node) request_msg.end_node = String(data=request.end_node) request_msg.duckie_id = String(data=request.duckie_id) msg.filled_requests.append(request_msg) for duckie_id in duckies: duckie = duckies[duckie_id] duckiestate_msg = DuckieState() duckiestate_msg.duckie_id = String(data=duckie_id) duckiestate_msg.status = String(data=duckie.status) duckiestate_msg.lane = String(data=duckie.next_point['lane']) duckiestate_msg.pose = Pose2D( x=duckie.pose.p[0] * self.state_manager.dt_map.tile_size, y=duckie.pose.p[1] * self.state_manager.dt_map.tile_size, theta=duckie.pose.theta) duckiestate_msg.velocity = Twist( linear=Vector3(duckie.velocity['linear'], 0, 0), angular=Vector3(0, 0, duckie.velocity['angular'])) duckiestate_msg.in_fov = [ String(data=visible_duckie) for visible_duckie in duckie.in_fov ] duckiestate_msg.collision_level = UInt8( data=duckie.collision_level) msg.duckie_states.append(duckiestate_msg) return msg
def callback(data): # data.ranges and data.intensities # index is from 0..3599, where # - 0 is rear # - 900 is (base's) right # - 1800 is front # - 2700 is (base's) left # Declaration of global variables modified in this method global publishCounter global objectAnglesPublish global objectDistancesPublish # Variables to store the edited values of angles and distances avgAnglesEdited = [] # from tenth of degrees to a degree avgDistancesEdited = [] # from m to cm publishCounter += 1 # only publish if the counter is equals to the rate we want to publish in if publishCounter == publishRate: obstructed(data, 950, 2650) # edit the data inside the arrays avgDistancesEdited = map(lambda item: item * 100, objectDistancesPublish) avgAnglesEdited = map(lambda item: item / 10, objectAnglesPublish) # print the data we're publishing print "===============" print "Avg Distances: ", print str(["{0:3.1f}".format(item) for item in avgDistancesEdited]).replace("'", "") print "Avg Angles : ", print str(["{0:3.1f}".format(item) for item in avgAnglesEdited]).replace("'", "") print "Object Found : ", print objectCounter # for index, item in enumerate(avgDistancesEdited) { # if item # } # define the type of message to send msgToSend = Float32MultiArray() objMsg = UInt32() # publish the angles msgToSend.data = avgAnglesEdited pubAngle.publish(msgToSend) # publish the distances msgToSend.data = avgDistancesEdited pubDistance.publish(msgToSend) # publish the number of objects objMsg.data = objectCounter numObjects.publish(objMsg) # restart the counter to 0 publishCounter = 0
def calculate_image_pose_offset(self, image_to_search_index, half_search_range=0): if self.last_image is not None: match_request = ImageMatchRequest( image_processing.image_to_msg(self.last_image), UInt32(image_to_search_index), UInt32(half_search_range)) match_response = self.match_image(match_request) # positive image offset: query image is shifted left from reference image # (normalise the pixel offset by the width of the image) return [ px_to_rad(offset) for offset in match_response.offsets.data ], match_response.correlations.data else: raise RuntimeError( 'Localiser: tried to localise before image data is received!')
def disable_cliff_sensors(self): # Experimentation shows that this message is unreliable # Better to continually publish it than hope it arrives once # flag to disable status LEDs is on by default (1) # flag to disable cliff reflex, from miro constants (21) msg = UInt32() msg.data = (1 << 1) + (1 << 21) self.pub_flags.publish(msg)
def handle_user_select(req): param_control_target_is_set = '/comm/param/control/target/is_set' param_control_user_selected = '/comm/param/control/target/selected' rsp = user_selectResponse() select_msg = String() num = req.select_num if num == 1 and fast_select: rsp.selected_id = 1 select_msg.data = 'Auto selected target No.1.' print select_msg.data pubInfo.publish(select_msg) return rsp selected = -1 info = "Please choose the one you want, if no target, enter 0." print info info_msg = String() info_msg.data = "CHOOSE" pubInfo.publish(info_msg) # Send this msg to APP for display info # Broadcast the target select request sr_msg = UInt32() sr_msg.data = num pubSR.publish(sr_msg) rospy.set_param(param_vision_search_param_lock, False) # unlock param change while selected < 0: if rospy.has_param(param_control_target_is_set): if not rospy.get_param(param_control_target_is_set): selected = 0 break if rospy.has_param(param_control_user_selected): selected = rospy.get_param(param_control_user_selected) if num >= selected > 0: select_msg.data = 'User has selected target No.' + str( selected) print select_msg.data pubInfo.publish(select_msg) break elif selected == 0: select_msg.data = 'No target selected.' print select_msg.data pubInfo.publish(select_msg) break else: selected = -1 rsp.selected_id = int(selected) # Reset the select parameter for next selection rospy.set_param(param_control_user_selected, -1) rospy.set_param(param_vision_search_param_lock, True) return rsp
def talker(): pub = rospy.Publisher('values', UInt32, queue_size=10) rospy.init_node('talker', anonymous=True) rate = rospy.Rate(20) # 20hz while not rospy.is_shutdown(): num = unsigned(randint(0,500)) rospy.loginfo(num) pub.publish(UInt32(num)) rate.sleep()
def cb_get_interests(self, request): interests_array = self.learning.get_normalized_interests_evolution() interests = GetInterestsResponse() if self.learning is not None: interests.names = self.learning.get_space_names() interests.num_iterations = UInt32(len(interests_array)) interests.interests = [ Float32(val) for val in interests_array.flatten() ] return interests
def RadiationSimulator(): rospy.init_node('RadiationSimulator', anonymous=True) #Publisher setup DwellTimePub = [] DwellTimePub.append(rospy.Publisher('/gamma1', UInt32, queue_size=10)) DwellTimePub.append(rospy.Publisher('/gamma2', UInt32, queue_size=10)) DwellTimePub.append(rospy.Publisher('/gamma3', UInt32, queue_size=10)) myRad = UInt32(1) #Listener setup listener = tf.TransformListener() sensorNames = ['/gamma1_tf', '/gamma2_tf', '/gamma3_tf'] sensorTimes = [time.time(), time.time(), time.time()] #Radiation source setup Radiation_Sources = [] Radiation_Sources.append( RadiationSource.RadiationSource(2, [0, 0], background_flag=True)) Radiation_Sources.append(RadiationSource.RadiationSource( 500, [0.75, 0.75])) #Control loop rate main_rate = 10 rate = rospy.Rate(main_rate) # 10hz while not rospy.is_shutdown(): #Simulate Radiation #Update DwellTime_Map each time through the loop for i in range(0, len(sensorNames)): try: (trans_gamma, rot_gamma) = listener.lookupTransform('map', sensorNames[i], rospy.Time(0)) #Update simulation data for each point Sim_Rad = SimulationRadation([trans_gamma[0], trans_gamma[1]], Radiation_Sources, time.time() - sensorTimes[i]) sensorTimes[i] = time.time() #Publish radiation for rad_i in range(0, Sim_Rad): DwellTimePub[i].publish(myRad) time.sleep(0.0001) #print("Gamma 1 CPS Updated") except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print(sensorNames[i] + " TF Exception in Radiation Simulator") pass
def main(): rospy.init_node("number_publisher") publisher = rospy.Publisher("number", UInt32, queue_size=10) n = UInt32() n.data = 0 while not rospy.is_shutdown(): n.data += 1 publisher.publish(n) rospy.sleep(1)
def epoc_publish_channels(): # Setup ROS publishers. signal_publishers = { channel: rospy.Publisher('epoc/signal/%s' % channel, UInt32) for channel in channels} quality_publishers = { channel: rospy.Publisher('epoc/quality/%s' % channel, UInt32) for channel in channels} # Open a connection to the Emotiv EPOC. headset = Emotiv() gevent.spawn(headset.setup) gevent.sleep(1) # Initialize ROS node+publisher. rospy.init_node('epoc_publish') # Start the publishing loop. rospy.loginfo('Starting publishing loop...') published_count = 0 try: while not rospy.is_shutdown(): # Get the next packet from the EPOC. packet = headset.dequeue() # Publish the the EEG channels and accelerometer values. for channel in channels: signal = UInt32(packet.sensors[channel]['value']) quality = UInt32(packet.sensors[channel]['quality']) signal_publishers[channel].publish(signal) quality_publishers[channel].publish(quality) # Update and print information count. published_count += 1 print('\rPublished: %d' % published_count, end='') gevent.sleep(0) except rospy.ROSInterruptException: headset.close()
def publish(self): if self.learning is not None: with self.lock_iteration: focus = copy(self.focus) ready = copy(self.ready_for_interaction) self.pub_ready.publish(Bool(data=ready)) self.pub_user_focus.publish( String(data=focus if focus is not None else "")) self.pub_focus.publish(String(data=self.learning.get_last_focus())) self.pub_iteration.publish( UInt32(data=self.learning.get_iterations()))
def __init__(self): #Initialize super().__init__("simulator_node") self.get_logger().info("INIT") self.pub_raw = self.create_publisher(Image, "/mopat/testbed/raw_image", 2) self.pub_starts = self.create_publisher(UInt32MultiArray, "/mopat/robot/robot_starts", 2) self.pub_goals = self.create_publisher(UInt32MultiArray, "/mopat/robot/robot_goals", 2) self.pub_num = self.create_publisher(UInt32, "/mopat/robot/robot_num", 2) #Class variables self.goals_multiarray = UInt32MultiArray( ) #Robot goals - UInt32MultiArray type rosmsg self.positions_multiarray = UInt32MultiArray( ) #Robot positions - UInt32MultiArray type rosmsg self.starts_multiarray = UInt32MultiArray( ) #Robot starts - UInt32MultiArray type rosmsg self.robot_num = UInt32() self.robot_starts = {} #Dict - robot_index : robot start self.robot_goals = {} #Dict - robot_index : robot goal self.steps = 50 #Simulation steps self.robot_index = 0 #Number of robots and indexing variable self.got_starts = False #Flag - True if user inputs all starts self.got_goals = False #Flag - True if user inputs all goals self.started = False #Flag - True if simulation started self.got_mouse_click = False #Flag - True if mouse clicked self.bridge = CvBridge() #Required for rosmsg-cv conversion self.robot_list = {} #Dict - robot_index : ith Robot object self.screen_size = (500, 500) #Default screen_size - (int, int) self.end_sim = False #Flag - Default ros param to end sim #Game initialization os.environ['SDL_VIDEO_WINDOW_POS'] = "+0,+50" pygame.init() pygame.display.set_caption("MoPAT Multi-Robot Simulator MkV") self.screen = pygame.display.set_mode(self.screen_size) self.draw_options = pymunk.pygame_util.DrawOptions(self.screen) self.clock = pygame.time.Clock() self.space = pymunk.Space() #Create map self.generate_random_map() #Get the simulator running in a way that the node can still receive messages #Run the simulator on a different thread while this nodes gets locked in spin run_thread = Thread(target=self.run) run_thread.daemon = True run_thread.start()
def parse_and_publish_state(self, state_object): time_now = rospy.get_rostime() # Joint state (position and velocity) ROS publisher joint_state_msg = JointState() joint_state_msg.header.stamp = time_now joint_state_msg.header.frame_id = "From real-time state data" joint_state_msg.name = JOINT_NAMES joint_state_msg.position = state_object.actual_q joint_state_msg.velocity = state_object.actual_qd # Tool state (position and velocity) ROS publisher tool_state_msg = ToolState() tool_state_msg.header.stamp = time_now tool_state_msg.header.frame_id = "From real-time state data" tool_pos = state_object.actual_TCP_pose tool_quat = hp.rotvec_to_quat(tool_pos[3:6]) tool_state_msg.position.position.x = tool_pos[0] tool_state_msg.position.position.y = tool_pos[1] tool_state_msg.position.position.z = tool_pos[2] tool_state_msg.position.orientation.x = tool_quat[0] tool_state_msg.position.orientation.y = tool_quat[1] tool_state_msg.position.orientation.z = tool_quat[2] tool_state_msg.position.orientation.w = tool_quat[3] tool_vel = state_object.actual_TCP_speed tool_state_msg.velocity.linear.x = tool_vel[0] tool_state_msg.velocity.linear.y = tool_vel[1] tool_state_msg.velocity.linear.z = tool_vel[2] tool_state_msg.velocity.angular.x = tool_vel[3] tool_state_msg.velocity.angular.y = tool_vel[4] tool_state_msg.velocity.angular.z = tool_vel[5] # End effector 6Dof Force wrench_msg = WrenchStamped() wrench_msg.header.stamp = time_now wrench_msg.wrench.force.x = state_object.actual_TCP_force[0] wrench_msg.wrench.force.y = state_object.actual_TCP_force[1] wrench_msg.wrench.force.z = state_object.actual_TCP_force[2] wrench_msg.wrench.torque.x = state_object.actual_TCP_force[3] wrench_msg.wrench.torque.y = state_object.actual_TCP_force[4] wrench_msg.wrench.torque.z = state_object.actual_TCP_force[5] runtime_state_msg = UInt32() runtime_state_msg.data = state_object.runtime_state self.pub_joint_states.publish(joint_state_msg) self.pub_tool_state.publish(tool_state_msg) self.pub_wrench.publish(wrench_msg)
def __init__(self): self.HOST = '192.168.1.200' self.PORT = 8080 self.BUFFER = 4096 self.RATE = rospy.Rate(10) self.pub = rospy.Publisher('tcptopic', String, queue_size=10) self.finish_sub = rospy.Subscriber('is_finished', Bool, self.finishCallback) self.check_sub = rospy.Subscriber('check', Int8, self.checkCallback) self.camera_sub = rospy.Subscriber('camera/color/camera_info', CameraInfo, self.cameraCallback) self.finish = False # is_finished? self.check = 0 # new!! checking joint state self.camera_check = False # new!! checking camera topic state self.camera_info = UInt32() # new!! checking camera topic state
def ticks_right_callback(self, data): #rospy.loginfo(rospy.get_caller_id() + " - ticks right: " + str(data.data)) if self.ticks_right == None: self.ticks_right = UInt32() self.ticks_right.data = data.data rospy.loginfo(rospy.get_caller_id() + " INITIAL - ticks right: " + str(data.data)) else: if self.wheel_power_right.data != 0.0 and \ data.data >= self.ticks_right.data + self.stop_wheel_ticks: self.wheel_power_right.data = 0.0 rospy.loginfo(rospy.get_caller_id() + " STOPPING - power right: " + str(self.wheel_power_right.data)) self.pub_velocity_right.publish(self.wheel_power_right) rospy.loginfo(rospy.get_caller_id() + " STOPPING - ticks right: " + str(data.data)) elif data.data >= self.ticks_right.data + self.stop_wheel_ticks: self.ticks_right.data = data.data
def _read(self, pathId, start, L): from math import ceil, floor N = int(ceil(abs(L) * self.frequency)) rospy.loginfo( "Start reading path {} (t in [ {}, {} ]) into {} points".format( pathId, start, start + L, N + 1)) self.reading = True self.queue = Queue.Queue(self.queue_size) times = (-1 if L < 0 else 1) * np.array(range(N + 1), dtype=float) / self.frequency times[-1] = L times += start self.firstMsgs = None for t in times: msgs = self.readAt(pathId, t, timeShift=start) if self.firstMsgs is None: self.firstMsgs = msgs self.pubs["read_path_done"].publish(UInt32(pathId)) rospy.loginfo("Finish reading path {}".format(pathId)) self.reading = False
def updateFirmware(self, node_id, file_path): pub_start = None pub_data = None pub_end = None if node_id == "power": pub_start = self._pub_power_ota_start pub_data = self._pub_power_ota_data pub_end = self._pub_power_ota_end else: pub_start = getattr(self, node_id)._pub_ota_start pub_data = getattr(self, node_id)._pub_ota_data pub_end = getattr(self, node_id)._pub_ota_end bytes_read = 0 file_size = os.path.getsize(file_path) pub_start.publish(UInt32(file_size)) self.sleep(1.0) with open(file_path) as f: numPacksSent = 0 while 1: bytes_to_read = 512 #4096 byte_s = f.read(bytes_to_read) if not byte_s: break ma = UInt8MultiArray() ma.data = [] for b in byte_s: bytes_read = bytes_read + 1 byte_int = int(b.encode('hex'), 16) ma.data.append(byte_int) pub_ota_data.publish(ma) self.sleep(0.050) pub_ota_end.publish(0) self.sleep(2)
def __init__(self): self.setup_opcua_server() self.pub_odom = rospy.Publisher('/odom', Odometry, queue_size=5) self.pub_status = rospy.Publisher('/robot_status', Int16, queue_size=5) self.pub_batv = rospy.Publisher('/voltage', Float32, queue_size=5) self.pub_toolstate = rospy.Publisher('/tool_state', Int16, queue_size=5) rospy.Subscriber('/cmd_vel', Twist, self.updateTwist) rospy.Subscriber('/enable_drives', Bool, self.updateEnable) rospy.Subscriber('/toggle_tool', Int16, self.updateTool) self.twist_data = Twist() self.odom = Odometry() self.enable = Bool() self.tool_state = Int16() self.connection = Bool() self.voltage = Float32() self.robot_status = Int16() self.i = UInt32()