From f28fd1c3e563fa3c441884b5da3de2a1a0f08427 Mon Sep 17 00:00:00 2001 From: "mona.goebel" <mona.goebel@tu-darmstadt.de> Date: Wed, 24 Apr 2024 15:17:50 +0200 Subject: [PATCH] Update ekf_navsat.yaml, pub_camGPS_msc2ros.py, and 17 more files... --- backpack_pkg/launch/backpack.launch | 46 +++- backpack_pkg/launch/backpack_sensors.launch | 16 +- backpack_pkg/params/FL_velodyne_vlp1.yaml | 10 +- backpack_pkg/params/camera_UserSet1.cfg | 111 ++++++++++ backpack_pkg/params/cameracalib.ini | 33 +++ backpack_pkg/params/cameracalib.yml | 26 +++ backpack_pkg/params/ekf_navsat.yaml | 125 ----------- backpack_pkg/params/gq7.yml | 100 +++++++++ ...tain_config.yml => microstrain_config.yml} | 66 ++++-- backpack_pkg/params/stcamera_node.yaml | 65 ++++++ backpack_pkg/src/cam_off.py | 23 -- backpack_pkg/src/cam_start_capture.py | 35 ---- .../src/overwrite_parameters_from_file.py | 84 ++++++++ backpack_pkg/src/pub_OdomInfo_ros2msc.py | 196 ------------------ backpack_pkg/src/pub_camGPS_msc2ros.py | 47 ----- backpack_pkg/src/pub_camInfo_msc2ros.py | 105 ---------- backpack_pkg/src/show_ROM_features.py | 40 ++++ backpack_pkg/src/sub_camera_piexif.py | 113 ++++++++++ 18 files changed, 672 insertions(+), 569 deletions(-) mode change 100755 => 100644 backpack_pkg/launch/backpack.launch mode change 100755 => 100644 backpack_pkg/launch/backpack_sensors.launch create mode 100644 backpack_pkg/params/camera_UserSet1.cfg create mode 100644 backpack_pkg/params/cameracalib.ini create mode 100644 backpack_pkg/params/cameracalib.yml delete mode 100644 backpack_pkg/params/ekf_navsat.yaml create mode 100644 backpack_pkg/params/gq7.yml rename backpack_pkg/params/{microstain_config.yml => microstrain_config.yml} (91%) create mode 100644 backpack_pkg/params/stcamera_node.yaml delete mode 100644 backpack_pkg/src/cam_off.py delete mode 100644 backpack_pkg/src/cam_start_capture.py create mode 100644 backpack_pkg/src/overwrite_parameters_from_file.py delete mode 100644 backpack_pkg/src/pub_OdomInfo_ros2msc.py delete mode 100644 backpack_pkg/src/pub_camGPS_msc2ros.py delete mode 100644 backpack_pkg/src/pub_camInfo_msc2ros.py create mode 100644 backpack_pkg/src/show_ROM_features.py create mode 100644 backpack_pkg/src/sub_camera_piexif.py diff --git a/backpack_pkg/launch/backpack.launch b/backpack_pkg/launch/backpack.launch old mode 100755 new mode 100644 index 50ddb1e..93b90ec --- a/backpack_pkg/launch/backpack.launch +++ b/backpack_pkg/launch/backpack.launch @@ -1,19 +1,47 @@ <launch> <arg name="record" default="false" /> - - <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp201" args="-0.02 0 0.83 3.14 0.785 0 /base_link /vlp201 100" /> - <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp202" args="-0.37 0 -0.15 0 -1.57 0 /base_link /vlp202 100" /> - <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="0.0 0 0.0 0 0 0 /base_link /imu_link 100" /> - <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss1" args="-0.1 0 0.83 0 0 0 /base_link /gnss1_link 10" /> - + <arg name="cam_connected" default="true" /> + + <!-- LIDAR --> + <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp201" args="-0.02 0 0.86 3.14 0.698 0 /base_link /vlp201 100" /> + <node pkg="tf" type="static_transform_publisher" name="base_link_to_vlp202" args="-0.37 0 -0.15 1.57 -1.57 1.57 /base_link /vlp202 100" /> + <!-- IMU --> + <node pkg="tf" type="static_transform_publisher" name="base_link_gq7_link_static_transform" args="0 0 0 0 0 0 base_link gq7_link 1000" /> + --> + <!-- GNSS maybe handled by MicroStrain + <node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss1" args="0 0 0.3 0 0 0 /sensor_wgs84_enu /base_link 200" /> + node pkg="tf" type="static_transform_publisher" name="base_link_to_gnss2" args="-0.01 -0.11 0.86 0 0 0 /base_link /gnss2_link 10" / --> + + <!-- map --> + <node pkg="tf" type="static_transform_publisher" name="map_to_base_link" args="0 0 0.3 0 0 0 /map /base_link 1000" /> + <node pkg="tf" type="static_transform_publisher" name="map_to_sensor_wgs84_enu" args="0 0 0 0 0 0 /map /earth 1000" /> <include file="$(find backpack_pkg)/launch/backpack_sensors.launch" > <arg name="record" value="$(arg record)" /> + <arg name="cam_connected" value="$(arg cam_connected)" /> </include> - <!-- node pkg="backpack_pkg" name="pub_camGPS_msc2ros" type="pub_camGPS_msc2ros.py" output="screen" /> - <node pkg="backpack_pkg" name="pub_camInfo_msc2ros" type="pub_camInfo_msc2ros.py" output="screen" / --> - <node pkg="backpack_pkg" name="inform_camera" type="pub_OdomInfo_ros2msc.py" output="screen" /> + + <!-- run ekf nodes: (1) EKF: odom > base_link (2) EKF: map > odom (depends on (1)); Navsat GPS > map> + <rosparam command="load" file="$(find backpack_pkg)/params/ekf_navsat.yaml" /> + + <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/> + + <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true"> + <remap from="odometry/filtered" to="odometry/filtered_map"/> + </node> --> + + + <!-- node pkg="backpack_pkg" name="pub_camGPS_msc2ros" type="pub_camGPS_msc2ros.py" output="screen" / --> + <!--node name="pub_camInfo_msc2ros" pkg="backpack_pkg" type="pub_camInfo_msc2ros.py" output="screen" ns="/" if="$(arg cam_connected)"/> + <node name="pub_OdomInfo_ros2msc" pkg="backpack_pkg" type="pub_OdomInfo_ros2msc.py" output="screen" ns="/" if="$(arg cam_connected)"/ --> + + + <!-- Node for the GPS attached on IMU > + <node pkg="robot_localization" type="navsat_transform_node" name="navsat_imu" clear_params="true" ns="imu"> + <remap from="odometry/filtered" to="odometry/filtered_map"/> + <remap from="gps/fix" to="/gnss1/fix"/> + </node --> <!-- include file="$(find backpack_pkg)/launch/laser_assemble.launch" / --> <!--include file="$(find backpack_pkg)/launch/gps_velo_pub.launch" /--> diff --git a/backpack_pkg/launch/backpack_sensors.launch b/backpack_pkg/launch/backpack_sensors.launch old mode 100755 new mode 100644 index 29ffe0b..95dc345 --- a/backpack_pkg/launch/backpack_sensors.launch +++ b/backpack_pkg/launch/backpack_sensors.launch @@ -1,5 +1,6 @@ <launch> <arg name="record" default="false" /> + <arg name="cam_connected" default="false" /> <group ns="vlp1"> <!-- Start the Velodyne driver (source to adjust two velodynes https://github.com/ros-drivers/velodyne/issues/108) --> @@ -30,10 +31,21 @@ </group> + <group ns="/"> + <!-- Start the camera driver --> + <node name="camera" pkg="omronsentech_camera" type="stcamera_node" output="screen"> + <!-- Load the default params file. --> + <rosparam file="$(find backpack_pkg)/params/stcamera_node.yaml" command="load" /> + </node> + + </group> + + + <!-- Start the bag file --> -<node pkg="rosbag" type="record" name="rosbag_record_imu_lidar" args="record -o $(find backpack_pkg)/bagfiles/backp_imu_lidar.bag -a" if="$(arg record)"/> - +<!-- node pkg="rosbag" type="record" name="rosbag_record_imu_lidar" args="record -o $(find backpack_pkg)/bagfiles/backp_imu_lidar.bag /vlp1/velodyne_points /vlp2/velodyne_points /nav/fix /imu/data /camera/dev_142122508751/camera_info /tf " if="$(arg record)"/ --> + <node pkg="rosbag" type="record" name="rosbag_record_imu_lidar" args="record -o $(find backpack_pkg)/bagfiles/backp_imu_lidar.bag -a" if="$(arg record)"/> </launch> diff --git a/backpack_pkg/params/FL_velodyne_vlp1.yaml b/backpack_pkg/params/FL_velodyne_vlp1.yaml index dcdbf43..b5ff0d1 100644 --- a/backpack_pkg/params/FL_velodyne_vlp1.yaml +++ b/backpack_pkg/params/FL_velodyne_vlp1.yaml @@ -2,7 +2,7 @@ common: lid_topic: "/vlp1/velodyne_points" imu_topic: "/imu/data" time_sync_en: false # ONLY turn on when external time synchronization is really not possible - time_offset_lidar_to_imu: -0.10873383 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + time_offset_lidar_to_imu: -0.002256 #-0.10873383 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 preprocess: @@ -20,10 +20,10 @@ mapping: fov_degree: 360 det_range: 100.0 extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, - extrinsic_T: [ 0.160012 -0.088218 0.757596] - extrinsic_R: [ 0.7207992, -0.0501856, 0.6913247, - 0.0694568, 0.9975849, 0.0000000, - -0.6896552, 0.0480172, 0.7225442 ] + extrinsic_T: [ 0 0.785 1.571] #extrinsic_T: [ 0.160012 -0.088218 0.757596] + extrinsic_R: [ -0.0001, -0.7074, 0.7068, #extrinsic_R: [ 0.7207992, -0.0501856, 0.6913247, + 1.0000, -0.0002, 0.0000, #0.0694568, 0.9975849, 0.0000000, + 0.0001, 0.7068, 0.7074 ] #-0.6896552, 0.0480172, 0.7225442 ] publish: path_en: true diff --git a/backpack_pkg/params/camera_UserSet1.cfg b/backpack_pkg/params/camera_UserSet1.cfg new file mode 100644 index 0000000..6d70130 --- /dev/null +++ b/backpack_pkg/params/camera_UserSet1.cfg @@ -0,0 +1,111 @@ +# {05D8C294-F295-4dfb-9D01-096BD04049F4} +# GenApi persistence file (version 3.4.0) +# RemoteDevice = Sentech::STC_U3V_CM -- Sentech Camera Interface -- Device version = 2.0.15 -- Product GUID = D0E8AF41-B99A-45D6-B660-6C23C4F5957F -- Product version GUID = 178CC30C-E80A-4AF4-A64C-5113725CA70B +DeviceLinkThroughputLimitMode {DeviceLinkSelector=0} Off +DeviceLinkSelector 0 +DeviceLinkThroughputLimit {DeviceLinkSelector=0} 398831548 +DeviceLinkSelector 0 +SensorShutterMode Global +BinningHorizontalMode {BinningSelector=Sensor} Average +BinningSelector Sensor +BinningHorizontal {BinningSelector=Sensor} 1 +BinningSelector Sensor +BinningVerticalMode {BinningSelector=Sensor} Average +BinningSelector Sensor +BinningVertical {BinningSelector=Sensor} 1 +BinningSelector Sensor +DecimationHorizontal 1 +DecimationVertical 1 +RegionMode {RegionSelector=Region1} Off +RegionMode {RegionSelector=Region2} Off +RegionMode {RegionSelector=Region3} Off +RegionMode {RegionSelector=Region4} Off +RegionMode {RegionSelector=Region5} Off +RegionMode {RegionSelector=Region6} Off +RegionMode {RegionSelector=Region7} Off +RegionSelector Region0 +Width {RegionSelector=Region0} 2048 +Width {RegionSelector=Region1} 0 +Width {RegionSelector=Region2} 0 +Width {RegionSelector=Region3} 0 +Width {RegionSelector=Region4} 0 +Width {RegionSelector=Region5} 0 +Width {RegionSelector=Region6} 0 +Width {RegionSelector=Region7} 0 +RegionSelector Region0 +Height {RegionSelector=Region0} 2048 +Height {RegionSelector=Region1} 0 +Height {RegionSelector=Region2} 0 +Height {RegionSelector=Region3} 0 +Height {RegionSelector=Region4} 0 +Height {RegionSelector=Region5} 0 +Height {RegionSelector=Region6} 0 +Height {RegionSelector=Region7} 0 +RegionSelector Region0 +OffsetX {RegionSelector=Region0} 0 +OffsetX {RegionSelector=Region1} 0 +OffsetX {RegionSelector=Region2} 0 +OffsetX {RegionSelector=Region3} 0 +OffsetX {RegionSelector=Region4} 0 +OffsetX {RegionSelector=Region5} 0 +OffsetX {RegionSelector=Region6} 0 +OffsetX {RegionSelector=Region7} 0 +RegionSelector Region0 +OffsetY {RegionSelector=Region0} 0 +OffsetY {RegionSelector=Region1} 0 +OffsetY {RegionSelector=Region2} 0 +OffsetY {RegionSelector=Region3} 0 +OffsetY {RegionSelector=Region4} 0 +OffsetY {RegionSelector=Region5} 0 +OffsetY {RegionSelector=Region6} 0 +OffsetY {RegionSelector=Region7} 0 +RegionSelector Region0 +PixelFormat {ImageComponentSelector=Intensity} Mono8 +ImageComponentSelector Intensity +ReverseX 0 +ReverseY 0 +MultiROIsInMultiPayloads Combined +AcquisitionFrameRate 20.0007 +TriggerMode {TriggerSelector=FrameStart} Off +TriggerMode {TriggerSelector=ExposureStart} Off +TriggerSelector FrameStart +TriggerSelector FrameStart +TriggerSelector FrameStart +ExposureMode Off +ExposureTimeSelector Common +ExposureAuto Off +LineMode {LineSelector=Line0} Input +LineMode {LineSelector=Line1} Input +LineMode {LineSelector=Line2} Input +LineSelector Line0 +LineInverter {LineSelector=Line0} 1 +LineInverter {LineSelector=Line1} 1 +LineInverter {LineSelector=Line2} 1 +LineSelector Line0 +LineSelector Line0 +UserOutputValue {UserOutputSelector=UserOutput0} 0 +UserOutputValue {UserOutputSelector=UserOutput1} 0 +UserOutputValue {UserOutputSelector=UserOutput2} 0 +UserOutputSelector UserOutput0 +StrobeOutDelay 30 +StrobeOutOnTime 32 +TriggerOutDelay 0 +TriggerOutOnTime 32 +LineDeviceResetMode Off +EventNotification {EventSelector=ExposureEnd} Off +EventSelector ExposureEnd +Gain {GainSelector=DigitalAll} 0 +GainSelector DigitalAll +DigitalGainOffsetMode On +GainAuto {GainSelector=DigitalAll} Off +GainSelector DigitalAll +GainSelector DigitalAll +GainSelector DigitalAll +GainSelector DigitalAll +BlackLevel {ImageComponentSelector=Intensity BlackLevelSelector=AnalogAll} 10 +ImageComponentSelector Intensity +BlackLevelSelector AnalogAll +ChunkModeActive 0 +ChunkEnable {ChunkSelector=Image} 1 +ChunkSelector Image +HDRMode Off diff --git a/backpack_pkg/params/cameracalib.ini b/backpack_pkg/params/cameracalib.ini new file mode 100644 index 0000000..0652220 --- /dev/null +++ b/backpack_pkg/params/cameracalib.ini @@ -0,0 +1,33 @@ +# produced by rosrun camera_calibration cameracalibrator.py --size 7x5 --square 0.95 image:=/stcamera_node/dev_142122508751/image_raw camera:=/stcamera_node/dev_142122508751 --no-service-check + +[image] + +width +2048 + +height +2048 + +[camera/dev_142122508751] + +camera matrix +1513.838016 0.000000 1036.847889 +0.000000 1505.835130 997.648866 +0.000000 0.000000 1.000000 + + +distortion +-0.161560 0.082874 0.000936 -0.000917 0.000000 + + +rectification +1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 0.000000 1.000000 + +projection +1420.022949 0.000000 1035.151324 0.000000 +0.000000 1411.911621 997.190455 0.000000 +0.000000 0.000000 1.000000 0.000000 + + diff --git a/backpack_pkg/params/cameracalib.yml b/backpack_pkg/params/cameracalib.yml new file mode 100644 index 0000000..3a3c7f7 --- /dev/null +++ b/backpack_pkg/params/cameracalib.yml @@ -0,0 +1,26 @@ +image_width: 2048 +image_height: 2048 +camera_name: camera/dev_142122508751 +camera_matrix: + rows: 3 + cols: 3 + data: [1513.83802, 0. , 1036.84789, + 0. , 1505.83513, 997.64887, + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.161560, 0.082874, 0.000936, -0.000917, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [1420.02295, 0. , 1035.15132, 0. , + 0. , 1411.91162, 997.19046, 0. , + 0. , 0. , 1. , 0. ] diff --git a/backpack_pkg/params/ekf_navsat.yaml b/backpack_pkg/params/ekf_navsat.yaml deleted file mode 100644 index ddc0ade..0000000 --- a/backpack_pkg/params/ekf_navsat.yaml +++ /dev/null @@ -1,125 +0,0 @@ - -ekf_local: - frequency: 10 - sensor_timeout: 0.4 - two_d_mode: false - transform_time_offset: 0.0 - transform_timeout: 0.0 - print_diagnostics: true - debug: false - - map_frame: map - odom_frame: odom - base_link_frame: base_link - world_frame: odom - - odom0: nav/odom - odom0_config: [true, true, true, - true, true, true, - false, false, false, - false, false, false, - false, false, false] - odom0_queue_size: 0 - odom0_nodelay: true - odom0_differential: false - odom0_relative: false - stamped_control: false - - #odom1: vlp2/slam/Odometry - #odom1_config: [true, true, true, - # true, true, true, - # false, false, false, - # false, false, false, - # false, false, false] - #odom1_queue_size: 0 - #odom1_nodelay: true - #odom1_differential: false - #odom1_relative: false - #stamped_control: false - - imu0: imu/data - imu0_config: [false, false, false, - true, true, true, - false, false, false, - true, true, true, - true, true, true] - imu0_nodelay: false - imu0_differential: false - imu0_relative: false - imu0_queue_size: 10 - imu0_remove_gravitational_acceleration: true - use_control: true - -ekf_global: - frequency: 100 - sensor_timeout: 0.4 - two_d_mode: false - transform_time_offset: 0.0 - transform_timeout: 0.0 - print_diagnostics: true - debug: false - - map_frame: map - odom_frame: odom - base_link_frame: base_link - world_frame: map - - odom0: nav/odom - odom0_config: [true, true, true, - true, true, true, - false, false, false, - false, false, false, - false, false, false] - odom0_queue_size: 0 - odom0_nodelay: true - odom0_differential: false - odom0_relative: false - stamped_control: false - - #odom1: vlp2/slam/Odometry - #odom1_config: [true, true, true, - # true, true, true, - # false, false, false, - # false, false, false, - # false, false, false] - #odom1_queue_size: 0 - #odom1_nodelay: true - #odom1_differential: false - #odom1_relative: false - #stamped_control: false - - odom2: ekf/odometry/gps #published by navsat_transform_node - odom2_config: [true, true, false, - false, false, false, - false, false, false, - false, false, false, - false, false, false] - odom2_queue_size: 0 - odom2_nodelay: true - odom2_differential: false - odom2_relative: false - - imu0: imu/data - imu0_config: [false, false, false, - true, true, true, - false, false, false, - true, true, true, - true, true, true] - imu0_nodelay: false - imu0_differential: false - imu0_relative: false - imu0_queue_size: 10 - imu0_remove_gravitational_acceleration: true - use_control: true - -navsat_transform: - frequency: 100 - delay: 3.0 - magnetic_declination_radians: 0.0551524 # For lat/long 49.862097999999996 N, 8.6790738 E - yaw_offset: 0 # IMU should read 0 for yaw when facing east. If not, set 1.570796327 (== 90°) if is reading 0 facing north - zero_altitude: false - broadcast_cartesian_transform: false - publish_filtered_gps: true - use_odometry_yaw: true - wait_for_datum: false - diff --git a/backpack_pkg/params/gq7.yml b/backpack_pkg/params/gq7.yml new file mode 100644 index 0000000..f669f76 --- /dev/null +++ b/backpack_pkg/params/gq7.yml @@ -0,0 +1,100 @@ +# You should change this section of config to match your setup +port : '/dev/microstrain_main' +baudrate : 115200 + +# This will cause the node to convert any NED measurements to ENU +# This will also cause the node to convert any vehicle frame measurements to the ROS definition of a vehicle frame +use_enu_frame : True + +# Configure some frame IDs +frame_id : 'gq7_link' # Frame ID of all of the filter messages. Represents the location of the CV7-INS in the tf tree +map_frame_id : "map" # Frame ID of the local tangent plane. +earth_frame_id : "earth" # Frame ID of the global frame +gnss1_frame_id : "gq7_gnss_1_antenna_link" +gnss2_frame_id : "gq7_gnss_2_antenna_link" +odometer_frame_id : "gq7_odometer_link" +target_frame_id : "base_link" # Frame ID that we will publish a transform to. + +# Disable the transform from the mount to frame id transform as it will be handled in the launch file +publish_mount_to_frame_id_transform : False + +# We will use relative transform mode, meaning that we will publish the following transforms from this node +# earth_frame_id -> map_frame_id +# map_frame_id -> target_frame_id +# This helps ROS standard tools consume and display position information produced by the device +tf_mode: 2 + +# Enable the RTK dongle interface for communication with the 3DM-RTK. +# Note: Even if you do not have a 3DM-RTK connected to the aux port, this boolean can remain true +rtk_dongle_enable : True + +# We will use base station relative position configuration for this example. This configuration does a couple things: +# We will setup a local tangent plane at the location of the RTK base station being used by the GQ7. +# We will publish this location as the transform from the earth_frame_id to map_frame_id frame +# Note: This configuration requires a 3DM-RTK to be connected to the aux port of the GQ7. +# If you do not have that device, see https://wiki.ros.org/microstrain_inertial_driver/relative_position_configuration for more options. +filter_relative_position_config : True +filter_relative_position_source : 2 + +# Set the antenna offsets. +# Note: These should be changed for you setup, otherwise dual antenna heading will not initialize. +gnss1_antenna_offset : [0.0, -0.7, -1.0] +gnss2_antenna_offset : [0.0, 0.7, -1.0] + +# This will set the heading alignment to be dual antenna +filter_auto_heading_alignment_selector : 1 + +# This will contain the raw IMU data, NOT the filtered IMU data +imu_data_rate : 100 + +# The default is to publish LLH position and velocity, but rviz has a hard time displaying that. +# Instead we will publish the position in the ECEF frame +gnss1_llh_position_data_rate : 0 +gnss1_velocity_data_rate : 0 +gnss1_odometry_earth_data_rate : 2 +gnss2_llh_position_data_rate : 0 +gnss2_velocity_data_rate : 0 +gnss2_odometry_earth_data_rate : 2 + +# We will only publish the odometry messages from the filter in this example. +# Also publish the human readable message which can be echoed from the command line +filter_human_readable_status_data_rate : 1 +filter_odometry_earth_data_rate : 10 +filter_odometry_map_data_rate : 10 + +# Turning on this will cause the transform between gq7_link and gnss_1_antenna_link/gnss_2_antenna_link to be updated from the filter +mip_filter_multi_antenna_offset_correction_data_rate : 2 + +################################################################################################################# +# Optional odometer config. Useful if using a wheeled vehicle with a wheel encoder connected to the GQ7 over GPIO +# This configuration will not cause any issues if you do not have an odometer connected +################################################################################################################# + +# Enable the hardware odometer +enable_hardware_odometer : True +odometer_scaling : 1.0 +odometer_uncertainty : 0.15 + +# Allow the filter to use the hardware odometer in it's estimates +filter_enable_odometer_aiding : True + +# For this example, we will assume that we are in a wheeled vehicle such as a car. +# Setting this option will give us better heading performance especially in GNSS outages +# If you are operating in a non wheeled vehicle make sure to disable this +filter_enable_wheeled_vehicle_constraint : True + +# Set the location of the odometer +# Note: These should be changed for you setup +filter_speed_lever_arm : [0.0, 0.0, 0.0] + +# This GPIO config is specific to this setup, and requires that you use GPIO pins 1 and 2 for encoder A and B respectively. +# You should update for your setup appropriately +gpio_config : True + +gpio1_feature : 3 # ENCODER feature +gpio1_behavior : 1 # ENCODER_A behavior +gpio1_pin_mode : 0 # NONE pin_mode + +gpio2_feature : 3 # ENCODER feature +gpio2_behavior : 2 # ENCODER_B behavior +gpio2_pin_mode : 0 # NONE pin_mode \ No newline at end of file diff --git a/backpack_pkg/params/microstain_config.yml b/backpack_pkg/params/microstrain_config.yml similarity index 91% rename from backpack_pkg/params/microstain_config.yml rename to backpack_pkg/params/microstrain_config.yml index 2076c29..4a434f4 100644 --- a/backpack_pkg/params/microstain_config.yml +++ b/backpack_pkg/params/microstrain_config.yml @@ -28,12 +28,12 @@ set_baud : False # # filter_frame_id and filter_child_frame_id are specifically useful as the node will also publish a transform to the /tf topic # that contains the transform between these two frames. Many ROS tools such as RViz will use the /tf topic to display things like robot position. -imu_frame_id : "imu_link_raw" +imu_frame_id : "gq7_link" gnss1_frame_id : "gnss1_link" gnss2_frame_id : "gnss2_link" -filter_frame_id : "imu_link" -filter_child_frame_id : "imu_link_raw" -nmea_frame_id : "nmea" +filter_frame_id : "map" +filter_child_frame_id : "gq7_odometer_link" +nmea_frame_id : "earth" # Waits for a configurable amount of time until the device exists # If poll_max_tries is set to -1 we will poll forever until the device exists @@ -41,6 +41,13 @@ poll_port : False poll_rate_hz : 1.0 poll_max_tries : 60 +# Number of times to attempt to reconnect to the device if it disconnects while running +# If configure_after_reconnect is true, we will also reconfigure the device after we reconnect +# NOTE: It is strongly recommended to configure after reconnect unless device_setup is set to false +# as the device will likely initialize in a different state otherwise +reconnect_attempts : 5 +configure_after_reconnect : True + # Controls if the driver outputs data with-respect-to ENU frame # false - position, velocity, and orientation are WRT the NED frame (native device frame) # true - position, velocity, and orientation are WRT the ENU frame @@ -54,18 +61,18 @@ device_setup : True # Controls if the driver-defined settings are saved # false - Do not save the settings # true - Save the settings in the device's non-volatile memory -save_settings : False +save_settings : True # Controls if the driver uses the device generated timestamp (if available) for timestamping messages # false - Use PC received time for timestamping # true - Use device generated timestamp -use_device_timestamp : False +use_device_timestamp : True # Controls if the driver uses ROS time for timestamping messages. Can be useful when running under simulation. # NOTE: This can be used in conjunction with use_device_timestamp in which case, the device timestmap will be used for sensor_msgs/TimeReference messages, and ROS time will be used for all other messages # false - Use PC received time for timestamping # true - Use ROS time for timestamping all non sensor_msgs/TimeReference messages -use_ros_time : False +use_ros_time : True # Controls if the driver creates a raw binary file # false - Do not create the file @@ -94,7 +101,7 @@ raw_file_directory : "/home/your_name" # ****************************************************************** # IMU Settings # ****************************************************************** -imu_data_rate : 100 +imu_data_rate : 200 # The speed at which the individual IMU publishers will publish at. # If set to -1, will use imu_data_rate to determine the rate at which to stream and publish @@ -107,9 +114,9 @@ imu_overrange_status_data_rate : -1 # Rate of imu/overrange_status topic # Static IMU message covariance values (the device does not generate these) # Since internally these are std::vector we need to use the rosparam tags -imu_orientation_cov : [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] -imu_linear_cov : [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] -imu_angular_cov : [0.01, 0, 0, 0, 0.01, 0, 0, 0, 0.01] +imu_orientation_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] +imu_linear_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] +imu_angular_cov : [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01] # ****************************************************************** # GNSS Settings (only applicable for devices with GNSS) @@ -134,7 +141,7 @@ gnss1_rf_error_detection_data_rate : -1 # Rate of gnss1/rf_error_detection topi # For GQ7 - in the vehicle frame wrt IMU origin (meters) # For all other models - in the IMU frame wrt IMU origin (meters) # Note: Make this as accurate as possible for good performance -gnss1_antenna_offset : [0.0, 0.16, 0.86] +gnss1_antenna_offset : [-0.015, -0.12, -0.85] # GNSS2 settings are only applicable for multi-GNSS systems (e.g. GQ7) gnss2_data_rate : 2 @@ -154,7 +161,12 @@ gnss2_rf_error_detection_data_rate : -1 # Rate of gnss1/rf_error_detection topi # Antenna #2 lever arm offset vector (In the vehicle frame wrt IMU origin (meters) ) # Note: Make this as accurate as possible for good performance -gnss2_antenna_offset : [0.0, -0.16, 0.86] +gnss2_antenna_offset : [-0.015, 0.12, -0.85] + +# GNSS signal configuration +gnss_glonass_enable : True +gnss_galileo_enable : True +gnss_beidou_enable : True # (GQ7 Only) Enable RTK dongle interface # Note: Enabling this will cause the node to publish on one of two topics depending @@ -183,7 +195,7 @@ publish_nmea : False # ****************************************************************** # Kalman Filter Settings (only applicable for devices with a Kalman Filter) # ****************************************************************** -filter_data_rate : 100 +filter_data_rate : 500 # The speed at which the individual Filter publishers will publish at. # If set to -1, will use filter_data_rate to determine the rate at which to stream and publish @@ -191,14 +203,18 @@ filter_data_rate : 100 # # Note: The parameters' "filter_odom_data_rate", "filter_imu_data_rate", "filter_relative_odom_data_rate" associated ROS messages share several MIP fields, # so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate +# Note: The parameters' "filter_navsatfix_data_rate", and "filter_aiding_status_data_rate" associated ROS messages share several MIP fields, +# so if more than one is set to stream, and they are set to stream at different rates, messages will be published on both topics, at the higher rate filter_status_data_rate : -1 # Rate of nav/status topic filter_heading_data_rate : -1 # Rate of nav/heading topic filter_heading_state_data_rate : -1 # Rate of nav/heading_state topic +filter_navsatfix_data_rate : -1 # Rate of nav/fix topic filter_odom_data_rate : -1 # Rate of nav/odom topic filter_imu_data_rate : -1 # Rate of nav/filtered_imu/data filter_relative_odom_data_rate : -1 # Rate of nav/relative_pos topic and the transform between filter_frame_id and filter_child_frame_id # Note that this data rate will also control the rate at which the transform between "filter_frame_id" and "filter_child_frame_id" will be published at filter_aiding_status_data_rate : -1 # Rate of gnss1/aiding_status and gnss2/aiding_status topics +filter_antenna_offset_correction_data_rate : -1 # Rate of gnss1/antenna_offset_corection and gnss2/antenna_offset_correction topics filter_gnss_dual_antenna_data_rate : -1 # Rate of nav/dual_antenna_status topic filter_aiding_measurement_summary_data_rate : -1 # Rate of nav/aiding_summary topic @@ -268,10 +284,11 @@ gps_leap_seconds : 18.0 # Notes: This subscription will be disabled if enable_hardware_odometer is set to true filter_external_speed_topic : "/external_speed" +# NOTE: This actually rotates the velocity into the sensor frame, not the vehicle frame. # Velocity frame control. # If set to false, the velocity will be reported in the NED/ENU frame # If set to true, the velocity will be reported in the vehicle frame -filter_vel_in_vehicle_frame : False +filter_vel_in_vehicle_frame : True # (GQ7 Only) Reference point lever arm offset control. # Note: This offset will affect the position and velocity measurements in the following topics: nav/odom, nav/relative_pos/odom @@ -363,7 +380,7 @@ gpio4_pin_mode : 0 # 2 - WGS84 Latitude, Longitude, height above ellipsoid position, NED velocity and attitude filter_init_condition_src : 0 filter_auto_heading_alignment_selector : 0,1 -filter_init_reference_frame : 2 +filter_init_reference_frame : 1 filter_init_position : [0.0, 0.0, 0.0] filter_init_velocity : [0.0, 0.0, 0.0] filter_init_attitude : [0.0, 0.0, 0.0] @@ -373,11 +390,16 @@ filter_init_attitude : [0.0, 0.0, 0.0] # 1 - Relative ECEF position # 2 - Relative LLH position # +# Source = +# 0 - Position will be reported relative to the base station. filter_relative_position_ref will be ignored +# 1 - Position will be reported relative to filter_relative_position_ref +# # Reference position - Units provided by reference frame (ECEF - meters, LLH - deg, deg, meters) # Note: prior to firmware version 1.0.06 this command will fail for non-positive heights. 1.0.06 fixes this) -filter_relative_position_config : False -filter_relative_position_frame : 2 -filter_relative_position_ref : [0, 0, 0.01] +filter_relative_position_config : True +filter_relative_position_frame : 1 +filter_relative_position_source : 2 +filter_relative_position_ref : [0.0, 0.0, 0.01] # (GQ7 only) Speed Lever Arm Configuration # Lever Arm - In vehicle reference frame (meters) @@ -392,11 +414,11 @@ filter_enable_wheeled_vehicle_constraint : False # Note: When enabled and no valid GNSS measurements are available, the filter uses the accelerometers to track # pitch and roll under the assumption that the sensor platform is not undergoing linear acceleration. # This constraint is useful to maintain accurate pitch and roll during GNSS signal outages. -filter_enable_vertical_gyro_constraint : False +filter_enable_vertical_gyro_constraint : True # (GQ7 only) GNSS Antenna Calibration Control # Note: When enabled, the filter will enable lever arm error tracking, up to the maximum offset specified in meters. -filter_enable_gnss_antenna_cal : False +filter_enable_gnss_antenna_cal : True filter_gnss_antenna_cal_max_offset : 0.1 # (GQ7/CV7 only) PPS Source @@ -428,7 +450,7 @@ nmea_message_allow_duplicate_talker_ids: False # NMEA messages in the sensor (IMU) descriptor set # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz # Note: In order to publish, 'publish_nmea' must also be set to True -imu_nmea_prkr_data_rate: 0 +imu_nmea_prkr_data_rate: 500 # NMEA messages in the GNSS1 descriptor set # In order to enable a message, set nmea_message_config to true, and then change the 'data_rate' of the sentences you want to the desired rate in hertz diff --git a/backpack_pkg/params/stcamera_node.yaml b/backpack_pkg/params/stcamera_node.yaml new file mode 100644 index 0000000..17a7c93 --- /dev/null +++ b/backpack_pkg/params/stcamera_node.yaml @@ -0,0 +1,65 @@ +# camera_to_connect: [values] +# values (case sensitive): +# ["all"] : connect to all detected camera +# [<MODEL(SERIAL)>, ...] : connect to only the specified camera with +# <MODEL(SERIAL)>. Model with serial number SN +# [<ID>, ...] : connect to only the specified camera with ID of ID +# (U3V GUID or hardware address of GigEVision camera) +# default value is empty (not specified). +# +# To connect to multiple cameras, separate the values with whitespace. +# If no value is specified, the first found camera will be used and the +# camera ID is used as camera namespace to identify the camera. +# If any value is specified, the specified name (either Model(SN) or ID) is used +# as camera namespace to identify the camera. +# +# The format of the camera namespace for the topics and services correspond to +# the connected camera is as follows: +# dev_{camera_id} or dev_{camera_model_serial_}, where: +# {camera_id} or {camera_model_serial_} are either camera ID or MODEL(SERIAL) +# with non-alphanumeric characters replaced with underscore "_". +# +# Example usage: +# camera_to_connect : ["all"] +# #namespace would be dev_{camera_id} of the connected camera. +# +# camera_to_connect : ["STC-MCS510U3V(00C0XXX)", "STC_SCS241POE(15DBXXX)"] +# #namespace would be: dev_STC_MCS510U3V_00C0XXX_ and STC_SCS241POE_15DBXXX_ +# +# camera_to_connect : ["142100030510", "00:11:1c:f6:yy:xx"] +# #namespace would be: dev_142100030510 and dev_00_11_1c_f6_yy_xx +# +# camera_to_connect : ["STC-MCS510U3V(00C0XX)", "00:11:1c:f6:yy:xx"] +# #namespace would be: dev_STC_MCS510U3V_00C0XXX_ and dev_00_11_1c_f6_yy_xx + +camera_to_connect : ["all"] + + +default_calibration_file: file:///home/deepforest/catkin_ws/src/backpack_pkg/params/cameracalib.yml +# value: +# <full path to the calibration file for any connected camera> +# default value is empty (not specified). +# +# if not specified, no calibration is performed. +# Please refer to http://wiki.ros.org/camera_calibration for the detail process +# for performing calibration. +# Tutorial for generating calibration file and rectifying image: +# http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration +# http://wiki.ros.org/camera_calibration_parsers +# http://wiki.ros.org/image_proc +# Example usage: +#default_calibration_file: file:///home/my_data/calibration/calibration.xml + + +# {camera_namespace}/calibration_file: value +# {camera_namespace} is determined based on the camera_to_connect values. Please +# refer to the camera_to_connect description above. +# value: +# The same format as default_calibration_file. +# default value is empty (not specified). +# +# If not specified, default_calibration_file is used. +# +# Example usage: +#dev_STC_MCS510U3V_00C0XXX_/calibration_file: file:///home/my_data/calibration/calibration_1.yaml +#dev_00_11_1c_f6_yy_xx/calibration_file: file:///home/my_data/calibration/calibration_2.xml diff --git a/backpack_pkg/src/cam_off.py b/backpack_pkg/src/cam_off.py deleted file mode 100644 index 8eb5d41..0000000 --- a/backpack_pkg/src/cam_off.py +++ /dev/null @@ -1,23 +0,0 @@ -import requests - -import rospy - - - -def end_capture(): - rospy.loginfo("Camera is ending auto capture.") - stop_capture = requests.post("http://192.168.1.83/timer?enable=false") - print(stop_capture.json()) - shutdown = requests.post("http://192.168.1.83/powerdownready?power_down=true") - print(shutdown.json()) - -if __name__ == '__main__': - try: - end_capture() - - except rospy.ROSInterruptException: - - pass - - - diff --git a/backpack_pkg/src/cam_start_capture.py b/backpack_pkg/src/cam_start_capture.py deleted file mode 100644 index 47c791e..0000000 --- a/backpack_pkg/src/cam_start_capture.py +++ /dev/null @@ -1,35 +0,0 @@ -import requests - -from argparse import ArgumentParser - -class StartEndCam: - - parser = ArgumentParser() - parser.add_argument("--autocapture", type=bool, help="Choose if camera auto capture mode starts or stops", default=False) - parser.add_argument("--shutdown", type=bool, help="Choose if camera should prepare for shutdown (detaches storage)", default=False) - args = parser.parse_args() - print(args.autocapture, args.shutdown) - - if args.autocapture == True: - # Post a message to the camera commanding a capture, block until complete - capture_params = { 'store_capture' : True, 'block' : True, 'use_post_capture_state': True , 'preview': True} ## use cached jpeg to show in rviz - config_params = { 'auto_cap_mode' : 'timer', 'timer_period' : 1.0} - requests.post("http://192.168.1.83/config", json=config_params) - requests.post("http://192.168.1.83/capture", json=capture_params) - capture = requests.post("http://192.168.1.83/timer?enable=true") - print(capture.json()) - while True: - status = requests.get("http://192.168.1.83/status") - print(status.json()) - - else: - stop_capture = requests.post("http://192.168.1.83/timer?enable=false") - print(stop_capture.json()) - - if args.shutdown ==True: - shutdown = requests.post("http://192.168.1.83/powerdownready?power_down=true") - print(shutdown.json()) - else: - pass - - diff --git a/backpack_pkg/src/overwrite_parameters_from_file.py b/backpack_pkg/src/overwrite_parameters_from_file.py new file mode 100644 index 0000000..53546e6 --- /dev/null +++ b/backpack_pkg/src/overwrite_parameters_from_file.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python + +""" + + This sample shows how to save/load camera setting with using featureBag. + + The following points will be demonstrated in this sample code: + + - Initialize StApi + + - Connect to camera + + - Save/load camera setting to/from file + + - Apply the loaded setting to camera + +""" + +import os +import stapipy as st + +file_path = os.path.join(os.path.dirname(__file__), '..', '..', 'camera_UserSet1.cfg') + +def set_enumeration(nodemap, enum_name, entry_name): + enum_node = st.PyIEnumeration(nodemap.get_node(enum_name)) + entry_node = st.PyIEnumEntry(enum_node[entry_name]) + enum_node.set_entry_value(entry_node) + +try: + # Initialize StApi before using. + st.initialize() + + # Create a system object for device scan and connection. + st_system = st.create_system() + + # Connect to first detected device. + st_device = st_system.create_first_device() + + # Display DisplayName of the device. + print('Device=', st_device.info.display_name) + + filename = file_path + + # Get the remote nodemap. + nodemap = st_device.remote_port.nodemap + + # Create feature bag object. + featurebag = st.create_featurebag() + + # Save the current settings to FeatureBag. + print("Saving current settings to FeatureBag...") + featurebag.store_file_to_bag(filename) + print("Saved current settings to FeatureBag") + + + # Load the UserSet that is stored in the ROM to the camera. + print("Loading settings from ROM to the camera...") + nodemap.get_node('UserSetLoad').get().execute() + print("Loaded settings from ROM") + + # Load the settings in the FeatureBag to the camera. + print("Loading settings from FeatureBag to the camera...") + featurebag.load(nodemap) + print("Loaded settings from FeatureBag") + + # Set the UserSet for UserSetSelector. + set_enumeration(nodemap, 'UserSetSelector', 'UserSet1') + + # Save the current settings to UserSet1. + print("Saving current settings to UserSet1...") + nodemap.get_node('UserSetSave').get().execute() + print("Saved current settings to UserSet1") + + # Set UserSet1 as the new default. + print("Setting UserSet1 as new default...") + set_enumeration(nodemap, 'UserSetDefault', 'UserSet1') + print("UserSet1 is set as new default") + + # Display all settings. + features = featurebag.save_to_string() + print(features) + +except Exception as exception: + print(exception) diff --git a/backpack_pkg/src/pub_OdomInfo_ros2msc.py b/backpack_pkg/src/pub_OdomInfo_ros2msc.py deleted file mode 100644 index b2ccb67..0000000 --- a/backpack_pkg/src/pub_OdomInfo_ros2msc.py +++ /dev/null @@ -1,196 +0,0 @@ -#!/usr/bin/env python - -import requests -from requests.adapters import HTTPAdapter -from requests.exceptions import ConnectionError - -import datetime -import math -import rospy -from nav_msgs.msg import Odometry -from std_msgs.msg import String - - -class ros2camera: - - def __init__(self): - - #for connection with ROS - rospy.init_node('inform_camera') - self.rate = rospy.Rate(1) - - #for connection to HTTP - self.t = 200 #timeout, to wait until connection is established, in seconds - self.adapter = HTTPAdapter(max_retries=5) # will retry a connection after it failed - self.session = requests.Session() - self.session.mount("http://192.168.1.83/", self.adapter) - try: - status = self.session.get("http://192.168.1.83/status", timeout=self.t) - rospy.loginfo(str(status.json())) - capture_params = { 'store_capture' : True, 'block' : True, 'use_post_capture_state': True , 'preview': True} - config_params = { 'auto_cap_mode' : 'timer', 'timer_period' : 1.0,} - self.session.post("http://192.168.1.83/config", json=config_params, timeout=self.t) - self.session.post("http://192.168.1.83/capture", json=capture_params, timeout=self.t) - capture = self.session.post("http://192.168.1.83/timer?enable=true", timeout=self.t) - rospy.loginfo("Camera is starting auto capture.") - rospy.loginfo(str(capture.json())) - except ConnectionError as ce: - rospy.logwarn(ce) - - while not rospy.is_shutdown() : - rospy.Subscriber("/nav/odom", Odometry, self.sending2camera) - self.pub = rospy.Publisher("/msc/msc_send_status", String, queue_size=10) - rospy.on_shutdown(self.end_capture) - - - def quaternion2euler(self, x,y,z,w): - t0 = +2.0 * (w * x + y * z) - t1 = +1.0 - 2.0 * (x * x + y * y) - roll_x = math.atan2(t0, t1) - - t2 = +2.0 * (w * y - z * x) - t2 = +1.0 if t2 > +1.0 else t2 - t2 = -1.0 if t2 < -1.0 else t2 - pitch_y = math.asin(t2) - - t3 = +2.0 * (w * z + x * y) - t4 = +1.0 - 2.0 * (y * y + z * z) - yaw_z = math.atan2(t3, t4) - - return roll_x, pitch_y, yaw_z # in radians - - - def sending2camera(self, data): - - # position - latitude = data.pose.pose.position.x - latitude = math.radians(latitude) - longitude = data.pose.pose.position.y - longitude = math.radians(longitude) - altitude = data.pose.pose.position.z - - #orientation - x = data.pose.pose.orientation.x - y = data.pose.pose.orientation.y - z = data.pose.pose.orientation.z - w = data.pose.pose.orientation.w - - phi, theta, psi = self.quaternion2euler(x,y,z,w) - - utc_time = datetime.datetime.utcnow() - utc_time = "{}-{:02d}-{:02d}T{}:{}:{}.{}".format(utc_time.year, utc_time.month, utc_time.day, utc_time.hour, utc_time.minute, utc_time.second, str(utc_time.microsecond)[:3]) - - - - ################# sending to camera - - position_params = { 'aircraft_phi' : phi, - 'aircraft_theta' : theta, - 'aircraft_psi' : psi, - 'camera_phi' : -1.5707963, - 'camera_theta' : -1.5707963, - 'camera_psi' : 0.0, - 'latitude' : latitude, - 'longitude': longitude, - 'altitude': altitude, - 'utc_time' : utc_time - } - rospy.loginfo("sending the information to the camera ...") - try: - capture_data_time = self.session.post("http://192.168.1.83/capture_state", json=position_params, timeout=self.t) - except ConnectionError as ce: - rospy.logwarn(ce) - txt = str(capture_data_time.json()) - self.pub.publish(txt) - txt = None - - - # orientation_params = { 'aircraft_phi' : phi, - # 'aircraft_theta' : theta, - # 'aircraft_psi' : psi, - # 'camera_phi' : -1.5707963, - # 'camera_theta' : -1.5707963, - # 'camera_psi' : 0, - - # } - # #print("sending the information to the camera ...") - # capture_data_time = requests.post("http://192.168.1.83/orientation", json=orientation_params) - # print(capture_data_time.json()) - - try: - cam_status = self.session.get("http://192.168.1.83/status", timeout=self.t) - except ConnectionError as ce: - rospy.logwarn(ce) - - self.pub.publish("Currently camera SD Storage left: ", str(cam_status.json()["sd_gb_free"])[:7], " / ", str(cam_status.json()["sd_gb_total"])[:7]) - cam_sd_red = cam_status.json()["sd_warn"] - if cam_sd_red == True: - rospy.logwarn("SD Card of red camera almost full!") - - self.rate.sleep() - - def end_capture(self): - rospy.loginfo("Camera is ending auto capture.") - try: - stop_capture = self.session.post("http://192.168.1.83/timer?enable=false", timeout=self.t) - txt = str(stop_capture.json()) - rospy.loginfo(txt) - txt = None - - except ConnectionError as ce: - rospy.logwarn(ce) - - -if __name__ == '__main__': - try: - ros2camera() - - except rospy.ROSInterruptException: - pass - - - - - -# ----------------- - - -''' -OUTPUT OF /imu/nav/odom - -header: - seq: 1671 - stamp: - secs: 1675513527 #TIME - nsecs: 360754406 - frame_id: "sensor_wgs84" -child_frame_id: "sensor" -pose: - pose: - position: - x: 49.862092389858105 #GPS lat - y: 8.679037934163667 #GPS LONG - z: 240.8105145785355 # GPS ALT - orientation: - x: 0.0018981278408318758 - y: -0.00014660877059213817 - z: 0.04816616699099541 - w: 0.9988375306129456 - covariance: [3582.8365157387598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3582.8365157387598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3582.8365157387598, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.973756077432382e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.9469057083309365e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0059109938767898385] -twist: - twist: - linear: - x: -0.15309178829193115 - y: 0.09651438146829605 - z: 0.2371663600206375 - angular: - x: 0.00015465420437976718 - y: 0.0018760506063699722 - z: -0.0005795593024231493 - covariance: [5.159766909499922, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 5.154747770559936, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.9731549150776715, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - - - -''' - - diff --git a/backpack_pkg/src/pub_camGPS_msc2ros.py b/backpack_pkg/src/pub_camGPS_msc2ros.py deleted file mode 100644 index 5c6c79c..0000000 --- a/backpack_pkg/src/pub_camGPS_msc2ros.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python - -import requests - -import rospy -#from rospy_message_converter import json_message_converter -from sensor_msgs.msg import NavSatFix - -pub = rospy.Publisher('gps', NavSatFix, queue_size=1) - -rospy.init_node('msc_gps_data') - -# Convert JSON information into ROS messages -def convertJSON2NavSatFix(data): - - latitude_b = float(data.json()['latitude']) - longitude_b = float(data.json()['longitude']) - altitude_b = float(data.json()['altitude']) - - return latitude_b, longitude_b, altitude_b - - -def talker(): - while not rospy.is_shutdown() : - gps_data = requests.get('http://192.168.1.83/gps') #Read the GPS data from the camera - json = convertJSON2NavSatFix(gps_data) - pub.publish(latitude=json[0], longitude=json[1], altitude=json[2]) - rospy.sleep(1.0) - - - -if __name__ == '__main__': - try: - talker() - except rospy.ROSInterruptException: pass - - -#### Raw Output of Camera JSON ### - -''' ->> 03.02.2023 -Raw data : {"altitude":0,"fix3d":false,"latitude":0,"longitude":0,"p_acc":0,"utc_time":"1970-01-01T00:00:00Z","utc_time_valid":false,"v_acc":0,"vel_d":0,"vel_e":0,"vel_n":0} - - -''' - - diff --git a/backpack_pkg/src/pub_camInfo_msc2ros.py b/backpack_pkg/src/pub_camInfo_msc2ros.py deleted file mode 100644 index 5302208..0000000 --- a/backpack_pkg/src/pub_camInfo_msc2ros.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python - -import requests -from requests.adapters import HTTPAdapter -from requests.exceptions import ConnectionError -import rospy - -from sensor_msgs.msg import CameraInfo - -class reportInfos(): - def __init__(self): - - pub_1 = rospy.Publisher('/msc/info_band1', CameraInfo, queue_size=1) - pub_2 = rospy.Publisher('/msc/info_band2', CameraInfo, queue_size=1) - pub_3 = rospy.Publisher('/msc/info_band3', CameraInfo, queue_size=1) - pub_4 = rospy.Publisher('/msc/info_band4', CameraInfo, queue_size=1) - pub_5 = rospy.Publisher('/msc/info_band5', CameraInfo, queue_size=1) - self.pub_list = [pub_1, pub_2, pub_3, pub_4, pub_5] - rospy.init_node('msc_bands') - - self.t = 2 #timeout, to wait until connection is established, in seconds - self.adapter = HTTPAdapter(max_retries=5) # will retry a connection after it failed - self.session = requests.Session() - self.session.mount("http://192.168.1.83/", self.adapter) - - # GET camera information and distortion - try: - self.capture_infos = self.session.get("http://192.168.1.83/camera_info", timeout=self.t) - self.capture_distortion = self.session.get("http://192.168.1.83/calibration/distortion", timeout=self.t) - self.talker() - except ConnectionError as ce: - rospy.logwarn(ce) - - - # Convert JSON information into ROS messages - def convertJSON2CameraInfo(self,cam_info, cam_dist, bandnumber): - ''' - height_1, width_1: image dimensions of band 1 - distortion_model_1: distortion model for band 1 used. Supported models are listed in sensor_msgs/distortion_models.h - D_plumb_bob_1 = plump blob distortion model for band 1. For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). - intrinsic_1 = Intrinsic camera matrix for the raw (distorted) images. - [fx 0 cx] - K = [ 0 fy cy] - [ 0 0 1] - Projects 3D points in the camera coordinate frame to 2D pixel coordinates using the focal lengths (fx, fy) and principal point (cx, cy). - ''' - b = str(bandnumber) - #header_b = capture_infos.json()[b]['bandname'] - height_b = int(cam_info.json()[b]['image_height']) - width_b = int(cam_info.json()[b]['image_width']) - distortion_model_b = 'plumb_bob'+'_'+cam_info.json()[b]['bandname'] - D_plumb_bob_b = [cam_dist.json()[b]['p'][0], cam_dist.json()[b]['p'][1], - cam_dist.json()[b]['k'][0], cam_dist.json()[b]['k'][1], cam_dist.json()[b]['k'][2]] - D_plumb_bob_b = [float(item) for item in D_plumb_bob_b] - - intrinsic_b = [cam_dist.json()[b]['fx'], 0, cam_dist.json()[b]['cx'], - 0, cam_dist.json()[b]['fy'], cam_dist.json()[b]['cy'], - 0,0,1] - intrinsic_b = [float(item) for item in intrinsic_b] - return height_b, width_b, distortion_model_b, D_plumb_bob_b, intrinsic_b - - - def talker(self): - - while not rospy.is_shutdown() : - for b in range(5): - json = self.convertJSON2CameraInfo(self.capture_infos, self.capture_distortion, b+1) - self.pub_list[b].publish(height=json[0], width=json[1], distortion_model=json[2], D=json[3], K=json[4]) - rospy.sleep(10.0) - - -if __name__ == '__main__': - - try: - reportInfos() - except rospy.ROSInterruptException: - pass - - -#### Raw Output of Camera JSON ### -''' ->> 03.02.2023 - --------------camera_info--------------- -{ -'1': {'bandname': 'Blue', 'bandwidth_nm': 32, 'center_nm': 475, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, -'2': {'bandname': 'Green', 'bandwidth_nm': 27, 'center_nm': 560, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, -'3': {'bandname': 'Red', 'bandwidth_nm': 14, 'center_nm': 668, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, -'4': {'bandname': 'NIR', 'bandwidth_nm': 57, 'center_nm': 842, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'}, -'5': {'bandname': 'Red edge', 'bandwidth_nm': 12, 'center_nm': 717, 'focal_length_px': 1466.666666666667, 'image_height': 960, 'image_width': 1280, 'type': 'bandpass'} -} -------------calibration/distortion---------------- -{ -'1: {'cx': 632.5075, 'cy': 488.2897, 'fx': 1451.736, 'fy': 1451.7795, 'k': [-0.1047797, 0.1936219, -0.1258823], 'p': [0.0009945083, -0.0002144772]}, -'2': {'cx': 628.5667, 'cy': 478.4457, 'fx': 1445.8885, 'fy': 1445.7964, 'k': [-0.1064943, 0.169645, -0.07656262], 'p': [0.001048874, -0.0004467681]}, -'3': {'cx': 634.9805, 'cy': 478.7468, 'fx': 1451.3264, 'fy': 1451.1871, 'k': [-0.1100523, 0.1903069, -0.1222212], 'p': [0.0003323937, -0.0008141776]}, -'4': {'cx': 642.1021, 'cy': 478.8695, 'fx': 1445.3117, 'fy': 1445.3297, 'k': [-0.1120625, 0.1891504, -0.1246147], 'p': [0.0005723328, -0.0002671686]}, -'5': {'cx': 627.0835, 'cy': 490.9755, 'fx': 1445.6292, 'fy': 1445.5382, 'k': [-0.1115623, 0.1922929, -0.1185325], 'p': [0.0005413496, 0.0002610356]} -} - - - -''' - - diff --git a/backpack_pkg/src/show_ROM_features.py b/backpack_pkg/src/show_ROM_features.py new file mode 100644 index 0000000..c0057b6 --- /dev/null +++ b/backpack_pkg/src/show_ROM_features.py @@ -0,0 +1,40 @@ +""" + This sample shows how to save/load camera setting with using featureBag. + The following points will be demonstrated in this sample code: + - Initialize StApi + - Connect to camera + - Save/load camera setting to/from file + - Apply the loaded setting to camera +""" + +import os +import tempfile + +import stapipy as st + +try: + # Initialize StApi before using. + st.initialize() + + # Create a system object for device scan and connection. + st_system = st.create_system() + + # Connect to first detected device. + st_device = st_system.create_first_device() + + # Display DisplayName of the device. + print('Device=', st_device.info.display_name) + + # Get the remote nodemap. + nodemap = st_device.remote_port.nodemap + + # Create feature bag object. + featurebag = st.create_featurebag() + featurebag.store_nodemap_to_bag(nodemap) + + # Display all settings. + features = featurebag.save_to_string() + print(features) + +except Exception as exception: + print(exception) diff --git a/backpack_pkg/src/sub_camera_piexif.py b/backpack_pkg/src/sub_camera_piexif.py new file mode 100644 index 0000000..74ab351 --- /dev/null +++ b/backpack_pkg/src/sub_camera_piexif.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python + +import rospy +from sensor_msgs.msg import Image, NavSatFix +from cv_bridge import CvBridge, CvBridgeError +import cv2 +import numpy as np +import os +from datetime import datetime +import piexif + +class ImageGPSProcessor: + def __init__(self, save_path): + rospy.init_node('image_gps_processor', anonymous=True) + + # Create folder name for all images + folder_time = datetime.now().strftime("%Y-%m-%d_%H-%M") + self.save_path = os.path.join(save_path, folder_time) + os.makedirs(self.save_path, exist_ok=True) + rospy.loginfo(f"Saving images here {str(self.save_path)}.") + + self.bridge = CvBridge() + + # Subscribe to image topic + self.image_sub = rospy.Subscriber("/camera/dev_142122508751/image_raw", Image, self.image_callback) + + # Subscribe to GPS topic + self.gps_sub = rospy.Subscriber("/nav/fix", NavSatFix, self.gps_callback) + + self.gps_info = None + + def image_callback(self, image_msg): + try: + # Convert ROS Image message to OpenCV image + cv_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough") + + # Extract timestamp from image message + timestamp_sec = image_msg.header.stamp.to_sec() + + # Save the image with a unique filename including milliseconds + image_id = image_msg.header.seq + image_name = f"image_with_gps_{image_id}.jpg" + image_path = os.path.join(self.save_path, image_name) + + # Save image as GeoTIFF + self.save_geotiff(cv_image, timestamp_sec, image_path) + + except CvBridgeError as e: + rospy.logerr(f"CV Bridge Error: {e}") + + def gps_callback(self, gps_msg): + # Store GPS information for later use + self.gps_info = gps_msg + + def save_geotiff(self, image, timestamp_sec, image_path): + if image is None: + rospy.logwarn("No image received...") + + else: + # Create the 0th dictionary + timestamp_dt = datetime.fromtimestamp(timestamp_sec) + zeroth_ifd = { + piexif.ImageIFD.Model: b"OMRON SENTECH STC-MBCM401U3V", + piexif.ImageIFD.XResolution: (2048, 1), + piexif.ImageIFD.YResolution: (2048, 1), + piexif.ImageIFD.DateTime: timestamp_dt.strftime("%Y-%m-%d_%H-%M-%S"), + piexif.ImageIFD.BitsPerSample: 8, # Mono8 + piexif.ImageIFD.ExposureTime: (11116, 1000000), # microseconds to rational + } + # Convert dictionary to EXIF format + exif_dict = {"0th": zeroth_ifd} + + if self.gps_info is not None: + # Extract GPS information + latitude = self.gps_info.latitude + longitude = self.gps_info.longitude + + # Create the GPS dictionary + gps_ifd = { + piexif.GPSIFD.GPSLatitudeRef: 'N' if latitude >= 0 else 'S', + piexif.GPSIFD.GPSLatitude: self.decimal_degrees_to_rational(abs(latitude)), + piexif.GPSIFD.GPSLongitudeRef: 'E' if longitude >= 0 else 'W', + piexif.GPSIFD.GPSLongitude: self.decimal_degrees_to_rational(abs(longitude)), + } + + exif_dict["GPS"] = gps_ifd + else: + rospy.logwarn("No GPS information available. Image will be saved without GPS metadata.") + + exif_bytes = piexif.dump(exif_dict) + + # Save image and insert EXIF data into the image + cv2.imwrite(image_path, image) + piexif.insert(exif_bytes, image_path) + + + + def decimal_degrees_to_rational(self, decimal_degrees): + degrees = int(decimal_degrees) + minutes_float = (decimal_degrees - degrees) * 60 + minutes = int(minutes_float) + seconds_float = (minutes_float - minutes) * 60 * 1000 # Multiply by 1000 to get milliseconds + seconds = int(seconds_float) + + # Rational format: (numerator, denominator) + return [(degrees, 1), (minutes, 1), (seconds, 1000)] + + def run(self): + rospy.spin() + +if __name__ == '__main__': + image_gps_processor = ImageGPSProcessor(save_path="/home/deepforest/catkin_ws/src/backpack_pkg/imagefiles") + image_gps_processor.run() -- GitLab