Giters

PX4 / PX4-Avoidance

PX4 avoidance ROS node for obstacle detection and avoidance.

Home Page:http://px4.io

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

Splitting The avoidance module so as to run the SITL on one Computer, and the avoidance api on another

a-sheikh opened this issue · comments

commented

I am trying to run a PX4 drone avoidance module, and a PX4 drone simulator, on two different computers. (intel x86, and jetson tx2). Both have ROS melodic installed.

To start, i modified the avoidance_sitl_mavros.launch, so all the files/parameters are hard coded. It runs, until it gets to the point of launching an empty rqt_reconfigure window.

simulation works on its own when i do make_px4_sitl default, and the sdf files are already generated before i run the launch file.

here is the output

aamir@aamir-HP:~$ roslaunch ~/PX4-Avoidance/avoidance/launch/avoidance_sitl_mavros.launch 
... logging to /home/aamir/.ros/log/71d72e70-d7f3-11eb-8a2c-00e04c680e2f/roslaunch-aamir-HP-29470.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://aamir-HP:36221/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /gazebo/enable_ros_network: True
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: ROLL_180
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: ROLL_180
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: udp://:14540@192....
 * /mavros/gcs_url: 
 * /mavros/global_position/frame_id: fcu
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: fcu_utm
 * /mavros/global_position/tf/frame_id: local_origin
 * /mavros/global_position/tf/send: False
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
 * /mavros/imu/frame_id: fcu
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/local_position/frame_id: local_origin
 * /mavros/local_position/tf/child_frame_id: fcu
 * /mavros/local_position/tf/frame_id: local_origin
 * /mavros/local_position/tf/send: True
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: False
 * /mavros/mocap/use_tf: True
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_throttle: False
 * /mavros/setpoint_attitude/tf/child_frame_id: attitude
 * /mavros/setpoint_attitude/tf/frame_id: local_origin
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 10.0
 * /mavros/setpoint_position/tf/child_frame_id: setpoint
 * /mavros/setpoint_position/tf/frame_id: local_origin
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/vibration/frame_id: vibration
 * /mavros/vision_pose/tf/child_frame_id: vision
 * /mavros/vision_pose/tf/frame_id: local_origin
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: melodic
 * /rosversion: 1.14.11
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    mavros (mavros/mavros_node)
    rqt_reconfigure (rqt_reconfigure/rqt_reconfigure)
    sitl_0 (px4/px4)
    spawn_model (gazebo_ros/spawn_model)

auto-starting new master
process[master]: started with pid [29480]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 71d72e70-d7f3-11eb-8a2c-00e04c680e2f
process[rosout-1]: started with pid [29491]
started core service [/rosout]
process[rqt_reconfigure-2]: started with pid [29499]
process[sitl_0-3]: started with pid [29503]
INFO  [px4] Creating symlink /home/aamir/Firmware/build/px4_sitl_default/etc -> /home/aamir/.ros/etc

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
process[mavros-4]: started with pid [29515]
Info: found model autostart file as SYS_AUTOSTART=1015
process[gazebo-5]: started with pid [29525]
INFO  [param] selected parameter default file eeprom/parameters_1015
[param] parameter file not found, creating eeprom/parameters_1015
process[spawn_model-6]: started with pid [29535]
  SYS_AUTOCONFIG: curr: 0 -> new: 1
[ INFO] [1624872616.507298431]: FCU URL: udp://:14540@192.168.2.1:14557
[ INFO] [1624872616.510438365]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1624872616.510732703]: udp0: Remote address: 192.168.2.1:14557
[ INFO] [1624872616.510991432]: GCS bridge disabled
[ INFO] [1624872616.527109766]: Plugin 3dr_radio loaded
[ INFO] [1624872616.529452083]: Plugin 3dr_radio initialized
[ INFO] [1624872616.529751641]: Plugin actuator_control loaded
[ INFO] [1624872616.533614391]: Plugin actuator_control initialized
[ INFO] [1624872616.539862776]: Plugin adsb loaded
  SYS_AUTOSTART: curr: 0 -> new: 1015
[ INFO] [1624872616.552847411]: Plugin adsb initialized
[ INFO] [1624872616.553037934]: Plugin altitude loaded
[ INFO] [1624872616.554306469]: Plugin altitude initialized
[ INFO] [1624872616.554510042]: Plugin cam_imu_sync loaded
[ INFO] [1624872616.555320082]: Plugin cam_imu_sync initialized
[ INFO] [1624872616.555492491]: Plugin command loaded
[ INFO] [1624872616.568617684]: Plugin command initialized
[ INFO] [1624872616.568920326]: Plugin companion_process_status loaded
[ INFO] [1624872616.574968676]: Plugin companion_process_status initialized
[ INFO] [1624872616.575226930]: Plugin debug_value loaded
[ INFO] [1624872616.586429618]: Plugin debug_value initialized
[ INFO] [1624872616.586488379]: Plugin distance_sensor blacklisted
[ INFO] [1624872616.586607599]: Plugin esc_status loaded
[ INFO] [1624872616.588098313]: Plugin esc_status initialized
[ INFO] [1624872616.588269095]: Plugin fake_gps loaded
  CAL_ACC0_ID: curr: 0 -> new: 1310988
[ INFO] [1624872616.600001179]: Plugin fake_gps initialized
[ INFO] [1624872616.600288561]: Plugin ftp loaded
[ INFO] [1624872616.616982395]: Plugin ftp initialized
[ INFO] [1624872616.617145666]: Plugin geofence loaded
[ INFO] [1624872616.621228012]: Plugin geofence initialized
[ INFO] [1624872616.621403898]: Plugin global_position loaded
  CAL_GYRO0_ID: curr: 0 -> new: 1310988
  CAL_ACC1_ID: curr: 0 -> new: 1310996
[ INFO] [1624872616.650829612]: Plugin global_position initialized
[ INFO] [1624872616.650976081]: Plugin gps_rtk loaded
[ INFO] [1624872616.654715406]: Plugin gps_rtk initialized
[ INFO] [1624872616.654853010]: Plugin gps_status loaded
[ INFO] [1624872616.657963432]: Plugin gps_status initialized
[ INFO] [1624872616.658125396]: Plugin hil loaded
  CAL_GYRO1_ID: curr: 0 -> new: 1310996
  CAL_ACC2_ID: curr: 0 -> new: 1311004
[ INFO] [1624872616.682816077]: Plugin hil initialized
[ INFO] [1624872616.682965124]: Plugin home_position loaded
  CAL_GYRO2_ID: curr: 0 -> new: 1311004
[ INFO] [1624872616.687454834]: Plugin home_position initialized
[ INFO] [1624872616.687615077]: Plugin imu loaded
  CAL_MAG0_ID: curr: 0 -> new: 197388
  CAL_MAG1_ID: curr: 0 -> new: 197644
[ INFO] [1624872616.701658561]: Plugin imu initialized
[ INFO] [1624872616.701808723]: Plugin landing_target loaded
  SENS_BOARD_X_OFF: curr: 0.0000 -> new: 0.0000
  SENS_DPRES_OFF: curr: 0.0000 -> new: 0.0010
[ INFO] [1624872616.718444553]: Plugin landing_target initialized
[ INFO] [1624872616.718776331]: Plugin local_position loaded
[ INFO] [1624872616.731687288]: Plugin local_position initialized
[ INFO] [1624872616.731840462]: Plugin log_transfer loaded
[ INFO] [1624872616.736085444]: Plugin log_transfer initialized
[ INFO] [1624872616.736260905]: Plugin manual_control loaded
[ INFO] [1624872616.745584761]: Plugin manual_control initialized
[ INFO] [1624872616.745758307]: Plugin mocap_pose_estimate loaded
[ INFO] [1624872616.750801901]: Plugin mocap_pose_estimate initialized
[ INFO] [1624872616.750964404]: Plugin mount_control loaded
[ INFO] [1624872616.756409228]: Plugin mount_control initialized
[ INFO] [1624872616.756676269]: Plugin obstacle_distance loaded
[ INFO] [1624872616.764282862]: Plugin obstacle_distance initialized
[ INFO] [1624872616.764465986]: Plugin odom loaded
[ INFO] [1624872616.772503932]: Plugin odom initialized
[ INFO] [1624872616.773028127]: Plugin onboard_computer_status loaded
[ INFO] [1624872616.779776674]: Plugin onboard_computer_status initialized
[ INFO] [1624872616.779938374]: Plugin param loaded
[ INFO] [1624872616.784692125]: Plugin param initialized
[ INFO] [1624872616.784848512]: Plugin play_tune loaded
[ INFO] [1624872616.789368121]: Plugin play_tune initialized
[ INFO] [1624872616.789583980]: Plugin px4flow loaded
[ INFO] [1624872616.813236025]: Plugin px4flow initialized
[ INFO] [1624872616.813406555]: Plugin rallypoint loaded
[ INFO] [1624872616.819916363]: Plugin rallypoint initialized
[ INFO] [1624872616.820095558]: Plugin rangefinder blacklisted
[ INFO] [1624872616.820282869]: Plugin rc_io loaded
[ INFO] [1624872616.835209023]: Plugin rc_io initialized
[ INFO] [1624872616.835263900]: Plugin safety_area blacklisted
[ INFO] [1624872616.835392974]: Plugin setpoint_accel loaded
[ INFO] [1624872616.839138024]: Plugin setpoint_accel initialized
[ INFO] [1624872616.839299860]: Plugin setpoint_attitude loaded
[ INFO] [1624872616.861231574]: Plugin setpoint_attitude initialized
[ INFO] [1624872616.863247138]: Plugin setpoint_position loaded
[ INFO] [1624872616.897285117]: Plugin setpoint_position initialized
[ INFO] [1624872616.897478501]: Plugin setpoint_raw loaded
[ INFO] [1624872616.910311720]: Plugin setpoint_raw initialized
[ INFO] [1624872616.910470987]: Plugin setpoint_trajectory loaded
[ INFO] [1624872616.921569412]: Plugin setpoint_trajectory initialized
[ INFO] [1624872616.922274570]: Plugin setpoint_velocity loaded
[ INFO] [1624872616.934881878]: Plugin setpoint_velocity initialized
[ INFO] [1624872616.935055932]: Plugin sys_status loaded
[ INFO] [1624872616.953544956]: Plugin sys_status initialized
[ INFO] [1624872616.953785077]: Plugin sys_time loaded
[ INFO] [1624872616.967628492]: TM: Timesync mode: MAVLINK
[ INFO] [1624872616.969549312]: Plugin sys_time initialized
[ INFO] [1624872616.969736947]: Plugin trajectory loaded
[ INFO] [1624872616.977631669]: Plugin trajectory initialized
[ INFO] [1624872616.977821082]: Plugin vfr_hud loaded
[ INFO] [1624872616.978847185]: Plugin vfr_hud initialized
[ INFO] [1624872616.978945884]: Plugin vibration blacklisted
[ INFO] [1624872616.979078548]: Plugin vision_pose_estimate loaded
[ INFO] [1624872617.000859341]: Plugin vision_pose_estimate initialized
[ INFO] [1624872617.001159105]: Plugin vision_speed_estimate loaded
[ INFO] [1624872617.008657199]: Plugin vision_speed_estimate initialized
[ INFO] [1624872617.009176342]: Plugin waypoint loaded
[ INFO] [1624872617.018477564]: Plugin waypoint initialized
[ INFO] [1624872617.018571581]: Plugin wheel_odometry blacklisted
[ INFO] [1624872617.018721019]: Plugin wind_estimation loaded
[ INFO] [1624872617.020353864]: Plugin wind_estimation initialized
[ INFO] [1624872617.020446874]: Autostarting mavlink via USB on PX4
[ INFO] [1624872617.020516523]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1624872617.020551848]: Built-in MAVLink package version: 2021.5.5
[ INFO] [1624872617.020593166]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1624872617.020624903]: MAVROS started. MY ID 1.240, TARGET ID 1.1
* SYS_AUTOCONFIG: curr: 1 -> new: 0
  IMU_INTEG_RATE: curr: 200 -> new: 250
[ INFO] [1624872617.340561375]: Finished loading Gazebo ROS API Plugin.
INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
[ INFO] [1624872617.345986425]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
PX4 SIM HOST: localhost
INFO  [simulator] Waiting for simulator to accept connection on TCP port 4560
[INFO] [1624872617.881938, 0.000000]: Loading model XML from Gazebo Model Database
[INFO] [1624872617.883276, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
Error Code: 11 Msg: Unable to find uri[model://lamp_with_lines]
Error Code: 11 Msg: Unable to find uri[model://lamp_with_lines]
Error Code: 11 Msg: Unable to find uri[model://powerline]
Error Code: 11 Msg: Unable to find uri[model://line_model]
[ INFO] [1624872706.530570731]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [1624872706.582094, 0.000000]: Calling service /gazebo/spawn_sdf_model
[ INFO] [1624872706.600529713]: Physics dynamic reconfigure ready.
Error Code: 11 Msg: Unable to find uri[model://iris_depth_camera]

<sdf version='1.6'>
  <world name='default'>
    <gravity>0 0 -9.8</gravity>
    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    <atmosphere type='adiabatic'/>
    <physics name='default_physics' default='0' type='ode'>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
    </physics>
    <scene>
      <ambient>0.4 0.4 0.4 1</ambient>
      <background>0.7 0.7 0.7 1</background>
      <shadows>1</shadows>
    </scene>
  </world>
</sdf>

[INFO] [1624872716.749604, 10.004000]: Spawn status: SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name iris_obs_avoid
[ERROR] [1624872716.758102, 10.004000]: Spawn service failed. Exiting.
[spawn_model-6] process has died [pid 29535, exit code 1, cmd /opt/ros/melodic/lib/gazebo_ros/spawn_model -sdf -database iris_depth_camera -model iris_obs_avoid __name:=spawn_model __log:=/home/aamir/.ros/log/71d72e70-d7f3-11eb-8a2c-00e04c680e2f/spawn_model-6.log].
log file: /home/aamir/.ros/log/71d72e70-d7f3-11eb-8a2c-00e04c680e2f/spawn_model-6*.log
[rqt_reconfigure-2] process has finished cleanly
log file: /home/aamir/.ros/log/71d72e70-d7f3-11eb-8a2c-00e04c680e2f/rqt_reconfigure-2*.log

Launch File

<launch>
    <arg name="world_path" default="/home/aamir/PX4-Avoidance/avoidance/sim/worlds/outdoor_village_3.world" />
    <arg name="gui" default="false"/>
    <arg name="ns" default="/"/>
    <arg name="model" default="iris_depth_camera"/>
    <arg name="fcu_url" default="udp://:14540@192.168.2.1:14557"/>
    <arg name="gcs_url" default="" />   <!-- GCS link is provided by SITL -->
    <arg name="tgt_system" default="1" />
    <arg name="tgt_component" default="1" />
    <arg name="vehicle" default="iris_obs_avoid"/>

    <param name="use_sim_time" value="true" />

    <!-- Launch rqt_reconfigure -->
    <node pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen" name="rqt_reconfigure" />

    <!-- Launch PX4 SITL -->
    <include file="$(find px4)/launch/px4.launch">
        <arg name="vehicle" value="$(arg vehicle)"/>
    </include>

    <!-- Launch MavROS -->
    <group ns="$(arg ns)">
        <include file="$(find mavros)/launch/node.launch">
            <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
            <!-- Need to change the config file to get the tf topic and get local position in terms of local origin -->
            <arg name="config_yaml" value="/home/aamir/PX4-Avoidance/avoidance/resource/px4_config.yaml" />
            <arg name="fcu_url" value="$(arg fcu_url)" />
            <arg name="gcs_url" value="$(arg gcs_url)" />
            <arg name="tgt_system" value="$(arg tgt_system)" />
            <arg name="tgt_component" value="$(arg tgt_component)" />
        </include>
    </group>

    <!-- Launch Gazebo -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="gui" value="$(arg gui)"/>
        <arg name="world_name" value="$(arg world_path)" />
    </include>

    <!-- Spawn vehicle model -->
    <node name="spawn_model" pkg="gazebo_ros" type="spawn_model" output="screen"
          args="-sdf -database $(arg model) -model $(arg vehicle)">
    </node>

</launch>