def sendRange(pub, radiation_type, r): msg = Range() msg.radiation_type = radiation_type msg.range = r pub.publish(msg)
#path_to_current_directory = str(pathlib.Path().parent.absolute()) #path_to_model = path_to_current_directory + "/models/" #loaded_model = pickle.load(open(path_to_model + model, 'rb')) loaded_model = pickle.load(open(path_to_model, 'rb')) canopy_position = {0: 'center', 1: 'left', 2: 'none', 3: 'right'} max_effective_spraying_distance = 0.8 # meters min_effective_height_percentage = 0.3 max_effective_height_percentage = 0.6 effective_width_percentage = 0.3 max_depth_distance = 14.0 # meters min_close_percentage = 0.3 fov_msg = Range() fov_msg.header.frame_id = "spray_valve_link" fov_msg.radiation_type = 1 fov_msg.field_of_view = 3.1415 / 3 fov_msg.min_range = 0 fov_msg.max_range = 1.0 valve_publisher = None fov_pub = None canopy_detected = False on_the_move = False valve = False def usbCamCallback(msg):
#!/usr/bin/env python from sensor_msgs.msg import Range import rospy topic = "test_range" publisher = rospy.Publisher(topic, Range) rospy.init_node("range_test") dist = 3 while not rospy.is_shutdown(): r = Range() r.header.frame_id = "base_link" r.header.stamp = rospy.Time.now() r.radiation_type = 0 r.field_of_view = 2.0 / dist r.min_range = 0.4 r.max_range = 10 r.range = dist publisher.publish(r) rospy.sleep(0.1) dist += 0.3 if dist > 10: dist = 3
def init_sensor_variables(self): #Primer mensaje #STD self.data_STD = Range() self.data_STD.radiation_type = self.data_STD.ULTRASOUND #Ultrasound self.data_STD.field_of_view = self.field_of_view #rad self.data_STD.min_range = self.sensor_min_range self.data_STD.max_range = self.sensor_max_range self.data_STD.range = self.sensor_max_range self.data_STD.header.stamp = rospy.Time.now() self.data_STD.header.frame_id = "STD_frame" #SDI self.data_SDI = Range() self.data_SDI.radiation_type = self.data_SDI.ULTRASOUND #Ultrasound self.data_SDI.field_of_view = self.field_of_view #rad self.data_SDI.min_range = self.sensor_min_range self.data_SDI.max_range = self.sensor_max_range self.data_SDI.range = float('Inf') self.data_SDI.header.stamp = rospy.Time.now() self.data_SDI.header.frame_id = "SDI_frame" #SLIT self.data_SLIT = Range() self.data_SLIT.radiation_type = self.data_SLIT.ULTRASOUND #Ultrasound self.data_SLIT.field_of_view = self.field_of_view #rad self.data_SLIT.min_range = self.sensor_min_range self.data_SLIT.max_range = self.sensor_max_range self.data_SLIT.range = self.sensor_max_range self.data_SLIT.header.stamp = rospy.Time.now() self.data_SLIT.header.frame_id = "SLIT_frame" #SLDD self.data_SLDD = Range() self.data_SLDD.radiation_type = self.data_SLDD.ULTRASOUND #Ultrasound self.data_SLDD.field_of_view = self.field_of_view #rad self.data_SLDD.min_range = self.sensor_min_range self.data_SLDD.max_range = self.sensor_max_range self.data_SLDD.range = self.sensor_max_range self.data_SLDD.header.stamp = rospy.Time.now() self.data_SLDD.header.frame_id = "SLDD_frame" #Segundo mensaje #SLDT self.data_SLDT = Range() self.data_SLDT.radiation_type = self.data_SLDT.ULTRASOUND #Ultrasound self.data_SLDT.field_of_view = self.field_of_view #rad self.data_SLDT.min_range = self.sensor_min_range self.data_SLDT.max_range = self.sensor_max_range self.data_SLDT.range = self.sensor_max_range self.data_SLDT.header.stamp = rospy.Time.now() self.data_SLDT.header.frame_id = "SLDT_frame" #STI self.data_STI = Range() self.data_STI.radiation_type = self.data_STI.ULTRASOUND #Ultrasound self.data_STI.field_of_view = self.field_of_view #rad self.data_STI.min_range = self.sensor_min_range self.data_STI.max_range = self.sensor_max_range self.data_STI.range = self.sensor_max_range self.data_STI.header.stamp = rospy.Time.now() self.data_STI.header.frame_id = "STI_frame" #SLID self.data_SLID = Range() self.data_SLID.radiation_type = self.data_SLID.ULTRASOUND #Ultrasound self.data_SLID.field_of_view = self.field_of_view #rad self.data_SLID.min_range = self.sensor_min_range self.data_SLID.max_range = self.sensor_max_range self.data_SLID.range = self.sensor_max_range self.data_SLID.header.stamp = rospy.Time.now() self.data_SLID.header.frame_id = "SLID_frame" #SDD self.data_SDD = Range() self.data_SDD.radiation_type = self.data_SDD.ULTRASOUND #Ultrasound self.data_SDD.field_of_view = self.field_of_view #rad self.data_SDD.min_range = self.sensor_min_range self.data_SDD.max_range = self.sensor_max_range self.data_SDD.range = self.sensor_max_range self.data_SDD.header.stamp = rospy.Time.now() self.data_SDD.header.frame_id = "SDD_frame" #Tercer Mensaje #SJO self.data_SJO = Range() self.data_SJO.radiation_type = self.data_SJO.ULTRASOUND #Ultrasound self.data_SJO.field_of_view = self.field_of_view #rad self.data_SJO.min_range = self.sensor_min_range self.data_SJO.max_range = self.sensor_max_range self.data_SJO.range = 1.0 self.data_SJO.header.stamp = rospy.Time.now() self.data_SJO.header.frame_id = "SJO_frame" #Arrays to calculate the median of the ultrasonic data self.max_array_ultrasonic = 3 #First message self.STD_array = [0] * self.max_array_ultrasonic self.SDI_array = [0] * self.max_array_ultrasonic self.SLIT_array = [0] * self.max_array_ultrasonic self.SLDD_array = [0] * self.max_array_ultrasonic self.i = 0 #Second message self.SLDT_array = [0] * self.max_array_ultrasonic self.STI_array = [0] * self.max_array_ultrasonic self.SLID_array = [0] * self.max_array_ultrasonic self.SDD_array = [0] * self.max_array_ultrasonic self.j = 0 #Third message self.SJO_array = [0] * self.max_array_ultrasonic self.k = 0
def run(self): # Connect to the ePuck self._bridge.connect() # Setup the necessary sensors. self.setup_sensors() # Disconnect when rospy is going to down rospy.on_shutdown(self.disconnect) self.greeting() self._bridge.step() # Subscribe to Commando Velocity Topic rospy.Subscriber("mobile_base/cmd_vel", Twist, self.handler_velocity) # Sensor Publishers # rospy.Publisher("/%s/mobile_base/" % self._name, ) if self.enabled_sensors['camera']: self.image_publisher = rospy.Publisher("camera", Image) if self.enabled_sensors['proximity']: for i in range(0, 8): self.prox_publisher.append( rospy.Publisher("proximity" + str(i), Range)) self.prox_msg.append(Range()) self.prox_msg[i].radiation_type = Range.INFRARED self.prox_msg[ i].header.frame_id = self._name + "/base_prox" + str(i) self.prox_msg[ i].field_of_view = 0.26 # About 15 degrees...to be checked! self.prox_msg[i].min_range = 0.005 # 0.5 cm self.prox_msg[i].max_range = 0.05 # 5 cm if self.enabled_sensors['motor_position']: self.odom_publisher = rospy.Publisher('odom', Odometry) if self.enabled_sensors['accelerometer']: self.accel_publisher = rospy.Publisher( 'accel', Imu) # Only "linear_acceleration" vector filled. if self.enabled_sensors['selector']: self.selector_publisher = rospy.Publisher('selector', Marker) if self.enabled_sensors['light']: self.light_publisher = rospy.Publisher('light', Marker) if self.enabled_sensors['motor_speed']: self.motor_speed_publisher = rospy.Publisher('motor_speed', Marker) if self.enabled_sensors['microphone']: self.microphone_publisher = rospy.Publisher('microphone', Marker) if self.enabled_sensors['floor']: self.floor_publisher = rospy.Publisher('floor', Marker) # Spin almost forever #rate = rospy.Rate(7) # 7 Hz. If you experience "timeout" problems with multiple robots try to reduce this value. self.startTime = time.time() while not rospy.is_shutdown(): self._bridge.step() self.update_sensors()
#!/usr/bin/env python import rospy import mavros_msgs #from mavros_msgs import srv from mavros_msgs.msg import State from sensor_msgs.msg import BatteryState, Temperature, Range drone_state = State() drone_battery = BatteryState() rangefinder = Range() def drone_validate(): imu_temperature = 25 # imu_temperature e a temperatura interna da pixhawk max_temperature = 60 rospy.init_node('validation_node') rate = rospy.Rate(20) rate.sleep() def state_callback(state_data): global drone_state drone_state = state_data def battery_callback(battery_data): global drone_battery drone_battery.voltage = battery_data.voltage drone_battery.current = battery_data.current
def __init__(self): # Initialize variables and Take off rospy.init_node('publish_test_data') # sonarOrientation = rospy.Subscriber('/tf', Tran) dataLeft = rospy.Publisher('/L_sensor', Range, queue_size=10) dataRight = rospy.Publisher('/R_sensor', Range, queue_size=10) dataFront = rospy.Publisher('/F_sensor', Range, queue_size=10) scan_pub = rospy.Publisher('/fakeScan', LaserScan, queue_size=10) self.sonarReadLeft = 20.0 self.sonarReadRight = 20.0 self.sonarReadFront = 20.0 rospy.Subscriber('/Left_sensor', Float32, self.setLeftSonarValue) rospy.Subscriber('/Right_sensor', Float32, self.setRightSonarValue) rospy.Subscriber('/Front_sensor', Float32, self.setFrontSonarValue) self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1) while not rospy.is_shutdown(): count = 0 # HRLV-MaxSonar-EZ MB1043 reports anything below 30 cm as 30 cm # inaccurate readings closer than 50 cm t = geometry_msgs.msg.TransformStamped() t.header.frame_id = "base_frame" t.header.stamp = rospy.Time.now() t.child_frame_id = "sonar_left" t.transform.translation.x = 0.0 t.transform.translation.y = 0.0 t.transform.translation.z = 0.0 t.transform.rotation.x = 0.0 t.transform.rotation.y = 0.0 t.transform.rotation.z = 0.0 t.transform.rotation.w = 1.0 leftSonar = Range() leftSonar.header.stamp = rospy.Time.now() leftSonar.header.frame_id = "sonar_left" leftSonar.radiation_type = Range.ULTRASOUND leftSonar.min_range = 20.0 leftSonar.max_range = 500.0 leftSonar.field_of_view = 0.4 leftSonar.range = self.sonarReadLeft #This value should be the readings from the actual sonars # print("publish data... ") tfm = tf2_msgs.msg.TFMessage([t]) self.pub_tf.publish(tfm) dataLeft.publish(leftSonar) t.header.stamp = rospy.Time.now() t.child_frame_id = "sonar_right" t.transform.translation.x = 0.0 t.transform.translation.y = 0.0 t.transform.translation.z = 0.0 t.transform.rotation.x = 0.0 t.transform.rotation.y = 0.0 t.transform.rotation.z = 1.0 t.transform.rotation.w = 0.0 tfm = tf2_msgs.msg.TFMessage([t]) self.pub_tf.publish(tfm) leftSonar.range = self.sonarReadRight leftSonar.header.frame_id = "sonar_right" dataRight.publish(leftSonar) t.header.stamp = rospy.Time.now() t.child_frame_id = "sonar_front" t.transform.translation.x = 0.0 t.transform.translation.y = 0.0 t.transform.translation.z = 0.0 t.transform.rotation.x = -0.5 t.transform.rotation.y = 0.5 t.transform.rotation.z = -0.5 t.transform.rotation.w = 0.5 leftSonar.range = self.sonarReadFront leftSonar.header.frame_id = "sonar_front" dataFront.publish(leftSonar) tfm = tf2_msgs.msg.TFMessage([t]) self.pub_tf.publish(tfm)
def poll(self): now = rospy.Time.now() if now > self.t_next: try: self.emergencybt_val = self.arduino.get_emergency_button() self.emergencybt_pub.publish(self.emergencybt_val) except: self.emergencybt_val = -1 self.emergencybt_pub.publish(-1) rospy.logerr("get_emergency_button except") return try: self.voltage_val = self.arduino.get_voltage() self.voltage_pub.publish(self.voltage_val) self.voltage_percentage_pub.publish( self.volTransPerentage(self.voltage_val)) #rospy.loginfo("voltage_Perentage:" + str(self.volTransPerentage(self.voltage_val))) except: self.voltage_pub.publish(-1) #self.voltage_str_pub.publish("") rospy.logerr("get voltage exception") if (self.useSonar == True): pcloud = PointCloud2() try: self.sonar_r0, self.sonar_r1 = self.arduino.ping_o() #rospy.loginfo("r0: " + str(self.sonar_r0) + " r1: " + str(self.sonar_r1)) sonar0_range = Range() sonar0_range.header.stamp = now sonar0_range.header.frame_id = "/sonar0" sonar0_range.radiation_type = Range.ULTRASOUND sonar0_range.field_of_view = 0.3 sonar0_range.min_range = 0.04 sonar0_range.max_range = 0.8 sonar0_range.range = self.sonar_r0 / 100.0 if sonar0_range.range == 0.0: #sonar error or not exist flag sonar0_range.range = 1.0 elif sonar0_range.range >= sonar0_range.max_range: #sonar0_range.range = float('Inf') sonar0_range.range = sonar0_range.max_range self.sonar0_pub.publish(sonar0_range) if sonar0_range.range >= 0.5 or sonar0_range.range == 0.0: self.sonar_cloud[0][ 0] = self.sonar0_offset_x + self.sonar_maxval * math.cos( self.sonar0_offset_yaw) self.sonar_cloud[0][ 1] = self.sonar0_offset_y + self.sonar_maxval * math.sin( self.sonar0_offset_yaw) else: self.sonar_cloud[0][ 0] = self.sonar0_offset_x + sonar0_range.range * math.cos( self.sonar0_offset_yaw) self.sonar_cloud[0][ 1] = self.sonar0_offset_y + sonar0_range.range * math.sin( self.sonar0_offset_yaw) #rospy.loginfo("r0: " + str(sonar0_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[0][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[0][1])) sonar1_range = Range() sonar1_range.header.stamp = now sonar1_range.header.frame_id = "/sonar1" sonar1_range.radiation_type = Range.ULTRASOUND sonar1_range.field_of_view = 0.3 sonar1_range.min_range = 0.04 sonar1_range.max_range = 0.8 sonar1_range.range = self.sonar_r1 / 100.0 if sonar1_range.range == 0.0: #sonar error or not exist flag sonar1_range.range = 1.0 elif sonar1_range.range >= sonar0_range.max_range: sonar1_range.range = sonar1_range.max_range self.sonar1_pub.publish(sonar1_range) if sonar1_range.range >= 0.5 or sonar1_range.range == 0.0: self.sonar_cloud[1][ 0] = self.sonar1_offset_x + self.sonar_maxval * math.cos( self.sonar1_offset_yaw) self.sonar_cloud[1][ 1] = self.sonar1_offset_y + self.sonar_maxval * math.sin( self.sonar1_offset_yaw) else: self.sonar_cloud[1][ 0] = self.sonar1_offset_x + sonar1_range.range * math.cos( self.sonar1_offset_yaw) self.sonar_cloud[1][ 1] = self.sonar1_offset_y + sonar1_range.range * math.sin( self.sonar1_offset_yaw) #rospy.loginfo("r1: " + str(sonar1_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[1][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[1][1])) except: self.bad_encoder_count += 1 rospy.logerr("ping_o exception count: " + str(self.bad_encoder_count)) return try: self.sonar_r2, self.sonar_r3 = self.arduino.ping_p() #rospy.loginfo("r2: " + str(self.sonar_r2) + " r3: " + str(self.sonar_r3)) sonar2_range = Range() sonar2_range.header.stamp = now sonar2_range.header.frame_id = "/sonar2" sonar2_range.radiation_type = Range.ULTRASOUND sonar2_range.field_of_view = 0.3 sonar2_range.min_range = 0.04 sonar2_range.max_range = 0.8 sonar2_range.range = self.sonar_r2 / 100.0 if sonar2_range.range == 0.0: #sonar error or not exist flag sonar2_range.range = 1.0 elif sonar2_range.range >= sonar2_range.max_range: sonar2_range.range = sonar2_range.max_range self.sonar2_pub.publish(sonar2_range) if sonar2_range.range >= 0.5 or sonar2_range.range == 0.0: self.sonar_cloud[2][ 0] = self.sonar2_offset_x + self.sonar_maxval * math.cos( self.sonar2_offset_yaw) self.sonar_cloud[2][ 1] = self.sonar2_offset_y + self.sonar_maxval * math.sin( self.sonar2_offset_yaw) else: self.sonar_cloud[2][ 0] = self.sonar2_offset_x + sonar2_range.range * math.cos( self.sonar2_offset_yaw) self.sonar_cloud[2][ 1] = self.sonar2_offset_y + sonar2_range.range * math.sin( self.sonar2_offset_yaw) #rospy.loginfo("r2: " + str(sonar2_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[2][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[2][1])) sonar3_range = Range() sonar3_range.header.stamp = now sonar3_range.header.frame_id = "/sonar3" sonar3_range.radiation_type = Range.ULTRASOUND sonar3_range.field_of_view = 0.3 sonar3_range.min_range = 0.04 sonar3_range.max_range = 0.8 sonar3_range.range = self.sonar_r3 / 100.0 if sonar3_range.range == 0.0: #sonar error or not exist flag sonar3_range.range = 1.0 elif sonar3_range.range >= sonar3_range.max_range: sonar3_range.range = sonar3_range.max_range self.sonar3_pub.publish(sonar3_range) if sonar3_range.range >= 0.5 or sonar3_range.range == 0.0: self.sonar_cloud[3][ 0] = self.sonar3_offset_x + self.sonar_maxval * math.cos( self.sonar3_offset_yaw) self.sonar_cloud[3][ 1] = self.sonar3_offset_y + self.sonar_maxval * math.sin( self.sonar3_offset_yaw) else: self.sonar_cloud[3][ 0] = self.sonar3_offset_x + sonar3_range.range * math.cos( self.sonar3_offset_yaw) self.sonar_cloud[3][ 1] = self.sonar3_offset_y + sonar3_range.range * math.sin( self.sonar3_offset_yaw) #rospy.loginfo("r3: " + str(sonar3_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[3][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[3][1])) except: self.bad_encoder_count += 1 rospy.logerr("ping_p exception count: " + str(self.bad_encoder_count)) return try: self.sonar_r4 = self.arduino.ping_q() #rospy.loginfo("r4: " + str(self.sonar_r4)) sonar4_range = Range() sonar4_range.header.stamp = now sonar4_range.header.frame_id = "/sonar4" sonar4_range.radiation_type = Range.ULTRASOUND sonar4_range.field_of_view = 0.3 sonar4_range.min_range = 0.04 sonar4_range.max_range = 0.8 sonar4_range.range = self.sonar_r4 / 100.0 if sonar4_range.range == 0.0: #sonar error or not exist flag sonar4_range.range = 1.0 elif sonar4_range.range >= sonar4_range.max_range: sonar4_range.range = sonar4_range.max_range self.sonar4_pub.publish(sonar4_range) if sonar4_range.range >= 0.5 or sonar4_range.range == 0.0: self.sonar_cloud[4][ 0] = self.sonar4_offset_x + self.sonar_maxval * math.cos( self.sonar4_offset_yaw) self.sonar_cloud[4][ 1] = self.sonar4_offset_y + self.sonar_maxval * math.sin( self.sonar4_offset_yaw) else: self.sonar_cloud[4][ 0] = self.sonar4_offset_x + sonar4_range.range * math.cos( self.sonar4_offset_yaw) self.sonar_cloud[4][ 1] = self.sonar4_offset_y + sonar4_range.range * math.sin( self.sonar4_offset_yaw) #rospy.loginfo("r4: " + str(sonar4_range.range) + " sonar_cloud_x: " + str(self.sonar_cloud[4][0])+ " sonar_cloud_y: " + str(self.sonar_cloud[4][1])) except: self.bad_encoder_count += 1 rospy.logerr("ping_q exception count: " + str(self.bad_encoder_count)) return #rospy.loginfo("r0: " + str(self.sonar_r0) + " r1: " + str(self.sonar_r1) + " r2: "+str(self.sonar_r2)+ " r3: " + str(self.sonar_r3)+ " r4: " + str(self.sonar_r4)) pcloud.header.frame_id = "/base_footprint" pcloud = pc2.create_cloud_xyz32(pcloud.header, self.sonar_cloud) self.sonar_pub_cloud.publish(pcloud) try: left_enc, right_enc = self.arduino.get_encoder_counts() #rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc)) self.lEncoderPub.publish(left_enc) self.rEncoderPub.publish(right_enc) except: self.bad_encoder_count += 1 rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) return dt = now - self.then self.then = now dt = dt.to_sec() # Calculate odometry if self.enc_left == None: dright = 0 dleft = 0 else: if (left_enc < self.encoder_low_wrap and self.enc_left > self.encoder_high_wrap): self.l_wheel_mult = self.l_wheel_mult + 1 elif (left_enc > self.encoder_high_wrap and self.enc_left < self.encoder_low_wrap): self.l_wheel_mult = self.l_wheel_mult - 1 else: self.l_wheel_mult = 0 if (right_enc < self.encoder_low_wrap and self.enc_right > self.encoder_high_wrap): self.r_wheel_mult = self.r_wheel_mult + 1 elif (right_enc > self.encoder_high_wrap and self.enc_right < self.encoder_low_wrap): self.r_wheel_mult = self.r_wheel_mult - 1 else: self.r_wheel_mult = 0 #dright = (right_enc - self.enc_right) / self.ticks_per_meter #dleft = (left_enc - self.enc_left) / self.ticks_per_meter dleft = 1.0 * (left_enc + self.l_wheel_mult * (self.encoder_max - self.encoder_min) - self.enc_left) / self.ticks_per_meter dright = 1.0 * (right_enc + self.r_wheel_mult * (self.encoder_max - self.encoder_min) - self.enc_right) / self.ticks_per_meter self.enc_right = right_enc self.enc_left = left_enc dxy_ave = (dright + dleft) / 2.0 dth = (dright - dleft) / self.wheel_track vxy = dxy_ave / dt vth = dth / dt if (dxy_ave != 0): dx = cos(dth) * dxy_ave dy = -sin(dth) * dxy_ave self.x += (cos(self.th) * dx - sin(self.th) * dy) self.y += (sin(self.th) * dx + cos(self.th) * dy) if (dth != 0): self.th += dth quaternion = Quaternion() quaternion.x = 0.0 quaternion.y = 0.0 quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # Create the odometry transform frame broadcaster. if (self.useImu == False): self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rospy.Time.now(), self.base_frame, "odom") odom = Odometry() odom.header.frame_id = "odom" odom.child_frame_id = self.base_frame odom.header.stamp = now odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = vxy odom.twist.twist.linear.y = 0 odom.twist.twist.angular.z = vth odom.pose.covariance = ODOM_POSE_COVARIANCE odom.twist.covariance = ODOM_TWIST_COVARIANCE self.odomPub.publish(odom) if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): self.v_des_left = 0 self.v_des_right = 0 if self.v_left < self.v_des_left: self.v_left += self.max_accel if self.v_left > self.v_des_left: self.v_left = self.v_des_left else: self.v_left -= self.max_accel if self.v_left < self.v_des_left: self.v_left = self.v_des_left if self.v_right < self.v_des_right: self.v_right += self.max_accel if self.v_right > self.v_des_right: self.v_right = self.v_des_right else: self.v_right -= self.max_accel if self.v_right < self.v_des_right: self.v_right = self.v_des_right self.lVelPub.publish(self.v_left) self.rVelPub.publish(self.v_right) # Set motor speeds in encoder ticks per PID loop if not self.stopped: self.arduino.drive(self.v_left, self.v_right) self.t_next = now + self.t_delta
#!/usr/bin/env python import rospy import math import tf from sensor_msgs.msg import LaserScan, Range from sensor_msgs.msg import Range Scan_msg = LaserScan() Range_msg = Range() LaserFrequency = 5 # frequency of scanning by range sensors. pub = rospy.Publisher('laser/laser_scan', LaserScan, queue_size=10) Scan_msg.header.frame_id = 'RangeLaserFrame' Scan_msg.angle_min = 0 Scan_msg.angle_max = 2*math.pi Scan_msg.angle_increment = 2*math.pi/6 #Scan_msg.scan_time = 0.5 Scan_msg.range_min = 0.10000000149 Scan_msg.range_max = 30 Scan_msg.intensities = [] RangesTable = [] for lA in range(0,8):
def __init__(self): rospy.init_node("range_array_converter", anonymous=True) # Init of variables self.tf_list = [] self.offsets = [] self.number_of_pos = 8 self.scan_init = False self.pt_cld_init = False self.number_of_sensors = 8 self.publishers = [] try: ns = rospy.get_namespace() if isinstance(ns, str): sep = '/' self.ns_prefix = sep.join( [x for x in (ns).split(sep) if len(x) > 0]) else: rospy.logwarn( "Error when fetching namespace, pefix \"\" will be used instead" ) except KeyError: self.ns_prefix = "" rospy.logwarn( "Error when fetching namespace, pefix \"\" will be used instead" ) # Getting mode param try: self.mode = rospy.get_param("~converter_mode") except KeyError: self.mode = "laser_scan" rospy.logwarn("Private parameter 'converter_mode' is not set." " Default value 'laser_scan' will be used instead.") self.range_array_topic = "ranges" # Getting sensor_mask try: self.sensor_mask = rospy.get_param("~sensor_mask") except KeyError: self.sensor_mask = [True] * self.number_of_sensors rospy.logwarn("Private parameter 'sensor_mask' is not set." " All sensors will be used for conversion") # Getting conversion frame try: self.conversion_frame = rospy.get_param("~conversion_frame") rospy.logwarn( "By specifying the conversion frame you will override the automatic namespacing of the conversion_frame" ) except KeyError: if self.ns_prefix != "": self.conversion_frame = "base_" + self.ns_prefix else: self.conversion_frame = "base_hub" rospy.logwarn( "Private parameter 'conversion_frame' is not set." " Default value 'base + _<namespace>' will be used instead.") # Getting refresh transform parameter try: self.force_tf_refresh = rospy.get_param("~force_tf_refresh") if self.force_tf_refresh: rospy.logwarn("WARNING: The transforms are going to be " "refreshed for each conversion. This may impact " "performance") except KeyError: self.force_tf_refresh = False rospy.logwarn("Private parameter 'force_tf_refresh' is not set." " Default value 'False' will be used instead.") # Creating the right publisher if self.mode == "laser_scan": self.publisher = rospy.Publisher("scan", LaserScan, queue_size=1) elif self.mode == "point_cloud": self.publisher = rospy.Publisher("point_cloud", PointCloud2, queue_size=1) elif self.mode == "sequential_ranges": self.publisher = rospy.Publisher("sequential_ranges", Range, queue_size=8) elif self.mode == "individual_ranges": for i in range(self.number_of_sensors): if self.sensor_mask[i]: self.publishers.append( rospy.Publisher("range_" + str(i), Range, queue_size=1)) else: self.publishers.append(None) else: rospy.logwarn("Incorrect value for the parameter 'converter_mode'." " Default value 'laser_scan' will be used instead.") self.mode = "laser_scan" self.publisher = rospy.Publisher("scan", LaserScan, queue_size=1) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) # Wait for tf before launching conversion rospy.loginfo('Waiting for tf to become available') if not self.tf_buffer.can_transform('base_link', 'base_link', rospy.Duration(0), rospy.Duration(60)): rospy.logerr('Tf timeout for transform between base_link and ' 'base_link') exit() rospy.Rate(0.33).sleep() rospy.loginfo('Tf successfully received') # Initializing callback self.hub_subscriber = rospy.Subscriber(self.range_array_topic, RangeArray, self.parser_callback) self.data = RangeArray() self.data.ranges = [Range()] * self.number_of_sensors
top_topic = "top_camera_range" front_topic = "front_camera_range" s_range = 4 fov = pi/4 top_pub = rospy.Publisher(top_topic, Range, queue_size = 10) front_pub = rospy.Publisher(front_topic, Range, queue_size = 10) rospy.init_node('range_publisher') r = rospy.Rate(10) while not rospy.is_shutdown(): h = Header() h.stamp = rospy.Time.now() h.frame_id = top_frame rg_top = Range() rg_top.header = h rg_top.field_of_view = fov rg_top.max_range = s_range rg_top.min_range = s_range rg_top.range = s_range rg_top.radiation_type = Range.INFRARED rg_front = copy.deepcopy(rg_top) rg_front.header.frame_id = front_frame top_pub.publish(rg_top) front_pub.publish(rg_front) r.sleep()
#!/usr/bin/env python import serial, time, rospy from sensor_msgs.msg import Range from std_msgs.msg import Header rospy.init_node('lidar_range') pub = [None, None, None, None] # 4개의 토픽 발행 for i in range(4): name = 'scan' + str(i) pub[i] = rospy.Publisher(name, Range, queue_size=1) msg = Range() h = Header() h.frame_id = "sensorXY" msg.header = h msg.radiation_type = Range().ULTRASOUND msg.min_range = 0.02 # 2cm msg.max_range = 2.0 # 2m msg.field_of_view = (30.0 / 180.0) * 3.14 while not rospy.is_shutdown(): msg.header.stamp = rospy.Time.now() # msg.range에 장애물까지의 거리를 미터 단위로 넣고 토픽을 발행한다. msg.range = 0.4 for i in range(4):
def init_sensor_variables(self): #Primer mensaje #STD self.data_STD=Range() self.data_STD.radiation_type=self.data_STD.ULTRASOUND #Ultrasound self.data_STD.field_of_view= self.field_of_view #rad self.data_STD.min_range=self.sensor_min_range self.data_STD.max_range=self.sensor_max_range self.data_STD.range=4.0 self.data_STD.header.stamp = rospy.Time.now() self.data_STD.header.frame_id = "STD_frame" #SDI self.data_SDI=Range() self.data_SDI.radiation_type=self.data_SDI.ULTRASOUND #Ultrasound self.data_SDI.field_of_view= self.field_of_view #rad self.data_SDI.min_range=self.sensor_min_range self.data_SDI.max_range=self.sensor_max_range self.data_SDI.range=1.4 self.data_SDI.header.stamp = rospy.Time.now() self.data_SDI.header.frame_id = "SDI_frame" #SLIT self.data_SLIT=Range() self.data_SLIT.radiation_type=self.data_SLIT.ULTRASOUND #Ultrasound self.data_SLIT.field_of_view= self.field_of_view #rad self.data_SLIT.min_range=self.sensor_min_range self.data_SLIT.max_range=self.sensor_max_range self.data_SLIT.range=3.0 self.data_SLIT.header.stamp = rospy.Time.now() self.data_SLIT.header.frame_id = "SLIT_frame" #SLDD self.data_SLDD=Range() self.data_SLDD.radiation_type=self.data_SLDD.ULTRASOUND #Ultrasound self.data_SLDD.field_of_view= self.field_of_view #rad self.data_SLDD.min_range=self.sensor_min_range self.data_SLDD.max_range=self.sensor_max_range self.data_SLDD.range=3.0 self.data_SLDD.header.stamp = rospy.Time.now() self.data_SLDD.header.frame_id = "SLDD_frame" #Segundo mensaje #SLDT self.data_SLDT=Range() self.data_SLDT.radiation_type=self.data_SLDT.ULTRASOUND #Ultrasound self.data_SLDT.field_of_view= self.field_of_view #rad self.data_SLDT.min_range=self.sensor_min_range self.data_SLDT.max_range=self.sensor_max_range self.data_SLDT.range=3.0 self.data_SLDT.header.stamp = rospy.Time.now() self.data_SLDT.header.frame_id = "SLDT_frame" #STI self.data_STI=Range() self.data_STI.radiation_type=self.data_STI.ULTRASOUND #Ultrasound self.data_STI.field_of_view= self.field_of_view #rad self.data_STI.min_range=self.sensor_min_range self.data_STI.max_range=self.sensor_max_range self.data_STI.range=3.0 self.data_STI.header.stamp = rospy.Time.now() self.data_STI.header.frame_id = "STI_frame" #SLID self.data_SLID=Range() self.data_SLID.radiation_type=self.data_SLID.ULTRASOUND #Ultrasound self.data_SLID.field_of_view= self.field_of_view #rad self.data_SLID.min_range=self.sensor_min_range self.data_SLID.max_range=self.sensor_max_range self.data_SLID.range=3.0 self.data_SLID.header.stamp = rospy.Time.now() self.data_SLID.header.frame_id = "SLID_frame" #SDD self.data_SDD=Range() self.data_SDD.radiation_type=self.data_SDD.ULTRASOUND #Ultrasound self.data_SDD.field_of_view= self.field_of_view #rad self.data_SDD.min_range=self.sensor_min_range self.data_SDD.max_range=self.sensor_max_range self.data_SDD.range=2.2 self.data_SDD.header.stamp = rospy.Time.now() self.data_SDD.header.frame_id = "SDD_frame" self.max_array_ultrasonic=3 self.STD_array=[0]*self.max_array_ultrasonic #Max 15 elements self.SDI_array=[0]*self.max_array_ultrasonic self.SLIT_array=[0]*self.max_array_ultrasonic self.SLDD_array=[0]*self.max_array_ultrasonic self.i=0 self.SLDT_array=[0]*self.max_array_ultrasonic self.STI_array=[0]*self.max_array_ultrasonic self.SLID_array=[0]*self.max_array_ultrasonic self.SDD_array=[0]*self.max_array_ultrasonic self.j=0
import rospy import traceback import sys import signal from VL53L0X.python.VL53L0X import VL53L0X from VL53L0X.python.VL53L0X import VL53L0X_BETTER_ACCURACY_MODE from sensor_msgs.msg import Range sys.path.append('.') rospy.init_node('distance_sensors') # public display name of the publisher rate = rospy.Rate(10) # 10hz range1Publisher = rospy.Publisher('/holly/range/front_left', Range, queue_size=5) range1Message = Range() range5Publisher = rospy.Publisher('/holly/range/front_right', Range, queue_size=5) range5Message = Range() range2Publisher = rospy.Publisher('/holly/range/rear', Range, queue_size=5) range2Message = Range() seq1 = 1 seq2 = 1 seq5 = 1 sensorSetupNeeded = 1
18 Vert spd Double Bestvel 8 73 19 Time Uint Time stamps 4 81 20 Crc Uint Crc 4 85 ''' ''' Field# Field name Field type Data description Bin bytes Bin Offest 1 Sync Ucahr 0x86 1 0 2 Data_lenth uchar 0x0a 1 1 3 Encoder_left short Encoder 2 2 4 Encoder_right Short Encoder 2 4 5 Time Uint Time stamps 4 6 6 Check_sum Uchar Checksum 1 10 ''' sound_range = Range() sound_seq = 0 def range_pub(range_msg): global sound_seq sound_range.max_range = 3.50 sound_range.min_range = 0.290 sound_range.field_of_view = 30 * math.pi / 180 sound_range.radiation_type = 0 sound_range.header.stamp = rospy.Time.now() sound_range.header.seq = sound_seq sound_range.header.frame_id = 'range1' sound_range.range = range_msg[0] / 1000.0 pub_range_1.publish(sound_range) sound_range.header.frame_id = 'range2'
def _receive_crazyflie_data(self, timestamp, data, logconf): # Catch all exceptions because otherwise the crazyflie # library catches them all silently try: stamp = self._calculate_timestamp(timestamp) if logconf.name == 'high_update_rate': imu = Imu() imu.header.stamp = stamp imu.header.frame_id = 'quad' imu.linear_acceleration.x = data['acc.x'] * 9.81 imu.linear_acceleration.y = data['acc.y'] * 9.81 imu.linear_acceleration.z = data['acc.z'] * 9.81 imu.orientation_covariance[0] = -1 imu.angular_velocity_covariance[0] = -1 variance = 6.0 imu.linear_acceleration_covariance[0] = variance imu.linear_acceleration_covariance[4] = variance imu.linear_acceleration_covariance[8] = variance self._imu_publisher.publish(imu) orientation = OrientationAnglesStamped() orientation.header.stamp = stamp orientation.data.roll = data[ 'stabilizer.roll'] * math.pi / 180.0 orientation.data.pitch = data[ 'stabilizer.pitch'] * math.pi / 180.0 # Zero out the yaw for now. Crazyflie has a problem with yaw calculation # and having correct yaw is not critical for this target yet. orientation.data.yaw = -0.0 * data[ 'stabilizer.yaw'] * math.pi / 180.0 self._orientation_pub.publish(orientation) if abs(orientation.data.roll) > 1.5 or abs( orientation.data.pitch) > 1.5: rospy.logwarn( 'Crazyflie FC Comms maximimum attitude angle exceeded') self._crash_detected = True elif logconf.name == 'medium_update_rate': twist = TwistWithCovarianceStamped() twist.header.stamp = stamp - rospy.Duration.from_sec(0.020) twist.header.frame_id = 'heading_quad' twist.twist.twist.linear.x = data['kalman_states.vx'] twist.twist.twist.linear.y = data['kalman_states.vy'] twist.twist.twist.linear.z = 0.0 variance = 0.0001 twist.twist.covariance[0] = variance twist.twist.covariance[7] = variance self._velocity_pub.publish(twist) range_msg = Range() range_msg.header.stamp = stamp - rospy.Duration.from_sec(0.050) range_msg.radiation_type = Range.INFRARED range_msg.header.frame_id = '/short_distance_lidar' range_msg.field_of_view = 0.3 range_msg.min_range = 0.01 range_msg.max_range = 1.6 self._current_height = data['range.zrange'] / 1000.0 range_msg.range = self._current_height if range_msg.range > 2.0: self._over_height_counts = self._over_height_counts + 1 else: self._over_height_counts = 0 if self._over_height_counts > 10: rospy.logwarn( 'Crazyflie FC Comms maximimum height exceeded') self._crash_detected = True self._range_pub.publish(range_msg) switch_msg = LandingGearContactsStamped() switch_msg.header.stamp = stamp pressed = data['range.zrange'] < 50 switch_msg.front = pressed switch_msg.back = pressed switch_msg.left = pressed switch_msg.right = pressed self._switch_pub.publish(switch_msg) elif logconf.name == 'slow_update_rate': battery = Float64Stamped() battery.header.stamp = stamp battery.data = data['pm.vbat'] self._vbat = data['pm.vbat'] self._battery_publisher.publish(battery) mag = MagneticField() mag.header.stamp = stamp mag.magnetic_field.x = data['mag.x'] mag.magnetic_field.y = data['mag.y'] mag.magnetic_field.z = data['mag.z'] self._mag_pub.publish(mag) else: rospy.logerr( 'Crazyflie FC Comms received message block with unkown name' ) except Exception as e: rospy.logerr("Crazyflie FC Comms error receiving data: {}".format( str(e))) rospy.logerr(traceback.format_exc())
ser.write(bytes(0)) #ser.write(0x00) ser.write(bytes(0)) #ser.write(0x01) ser.write(bytes(1)) #ser.write(0x06) ser.write(bytes(6)) Dist_Total = 0 Dist_L = 0 Dist_H = 0 distance = Range() pub = rospy.Publisher('sonar_frontR_distance', Range, queue_size=10) rospy.init_node('Lidar', anonymous=True) while (True and not rospy.is_shutdown()): while (ser.in_waiting >= 9 and not rospy.is_shutdown()): #print (ser.read()) #time.sleep(0.1) if ((b'Y' == ser.read()) and (b'Y' == ser.read())): GPIO.output(LEDpin, GPIO.LOW) Dist_L = ser.read() Dist_H = ser.read() Dist_Total = (ord(Dist_H) * 256) + (ord(Dist_L))
#!/usr/bin/env python import rospy from sensor_msgs.msg import Range ultrasound = Range() def ultrassond_callback(data): global ultrasound ultrasound.range = data.range rospy.init_node('range_printer', anonymous=True) rate = rospy.Rate(20) ultrasound_sub = rospy.Subscriber('/mavros/distance_sensor/hrlv_ez4_pub', Range, ultrassond_callback) while not rospy.is_shutdown(): rospy.loginfo(ultrasound.range) rate.sleep()
import numpy as np import cv2 from numpy import * import rospy from geometry_msgs.msg import PoseWithCovarianceStamped from sensor_msgs.msg import Imu ,Range import tf rospy.init_node('optical_flow', anonymous=False) poseMsg = PoseWithCovarianceStamped() imuMsg = Imu() rangeMsg = Range() x=0.0 y=0.0 cap = cv2.VideoCapture(1) R = [[0 for x in range(3)] for x in range(3)] def imucallback(data): quaternion = ( data.orientation.x, data.orientation.y, data.orientation.z, data.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1]
def __init__(self, conf={}): """STDRCircularSys.__init__ Arguments: - conf: configuration dictionary -- mass: point _mass_ -- sysdim: dimension of system, usually 1,2, or 3D -- statedim: 1d pointmass has 3 variables (pos, vel, acc) in this model, so sysdim * 3 -- dt: integration time step -- x0: initial state -- order: NOT IMPLEMENT (control mode of the system, order = 0 kinematic, 1 velocity, 2 force) """ SMPSys.__init__(self, conf) # state is (pos, vel, acc).T # self.state_dim if not hasattr(self, 'x0'): self.x0 = np.zeros((self.dim_s_proprio, 1)) self.x = self.x0.copy() self.cnt = 0 if not self.ros: import sys print( "ROS not configured but this robot (%s) requires ROS, exiting" % (self.__class__.__name__)) sys.exit(1) # timing self.rate = rospy.Rate(1 / self.dt) # pub / sub # actuator / motors self.twist = Twist() self.twist.linear.x = 0 self.twist.linear.y = 0 self.twist.linear.z = 0 self.twist.angular.x = 0 self.twist.angular.y = 0 self.twist.angular.z = 0 # odometry self.odom = Odometry() self.sonars = [] self.outdict = { "s_proprio": np.zeros((self.dim_s_proprio, 1)), "s_extero": np.zeros((self.dim_s_extero, 1)) } self.subs["odom"] = rospy.Subscriber("/robot0/odom", Odometry, self.cb_odom) for i in range(3): self.sonars.append(Range()) self.subs["sonar%d" % i] = rospy.Subscriber( "/robot0/sonar_%d" % i, Range, self.cb_range) self.pubs["cmd_vel"] = rospy.Publisher("/robot0/cmd_vel", Twist, queue_size=2) # reset robot self.reset() self.smdict["s_proprio"] = np.zeros((self.dim_s_proprio, 1)) self.smdict["s_extero"] = np.zeros((self.dim_s_extero, 1))
#!/usr/bin/env python from __future__ import division import rospy import VL53L1X from sensor_msgs.msg import Range rospy.init_node('vl53l1x') # range_pub = rospy.Publisher('~range', Range, queue_size=5) # TODO: why remmaping is not working? range_pub = rospy.Publisher('mavros/distance_sensor/rangefinder_3_sub', Range, queue_size=10) msg = Range() msg.radiation_type = Range.INFRARED msg.field_of_view = 0.471239 msg.min_range = 0 msg.max_range = 4 msg.header.frame_id = 'rangefinder' tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29) tof.open() # Initialise the i2c bus and configure the sensor tof.start_ranging( 3) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range rospy.loginfo('vl53l1x: start ranging') r = rospy.Rate(14) while not rospy.is_shutdown():