gsilano/BebopS

Problem when implementing robot swarm

Closed this issue · 6 comments

sheldonjinqi commented

Hi,
I am trying to use the BebopS to run some experiments on a team of robots. Right now I am having trouble with launching multiple robots and keep them at hovering.

This is the error i'm getting when i try to roslaunch my launch file. It seems like the gazebo node is publishing two topics "motor speed" with different data types. Any suggestions on how to fix this?

`started roslaunch server http://sheldon-Thinkpad:37461/

SUMMARY

PARAMETERS

  • /bebop1/position_controller_node/EKFActive: False
  • /bebop1/position_controller_node/H: 0.01
  • /bebop1/position_controller_node/Qp/aa: 0.001
  • /bebop1/position_controller_node/Qp/bb: 0.001
  • /bebop1/position_controller_node/Qp/cc: 0.001
  • /bebop1/position_controller_node/Qp/dd: 0.001
  • /bebop1/position_controller_node/Qp/ee: 0.001
  • /bebop1/position_controller_node/Qp/ff: 0.001
  • /bebop1/position_controller_node/Tsf: 1.5
  • /bebop1/position_controller_node/U_xyz/U_x: 1.181
  • /bebop1/position_controller_node/U_xyz/U_y: 1.181
  • /bebop1/position_controller_node/U_xyz/U_z: 4.6697
  • /bebop1/position_controller_node/beta_phi/beta_phi: -1.7514
  • /bebop1/position_controller_node/beta_psi/beta_psi: -14.3431
  • /bebop1/position_controller_node/beta_theta/beta_theta: -1.7513
  • /bebop1/position_controller_node/beta_xy/beta_x: -26.4259
  • /bebop1/position_controller_node/beta_xy/beta_y: -26.3627
  • /bebop1/position_controller_node/beta_z/beta_z: -27.2277
  • /bebop1/position_controller_node/bf: 8.54858e-06
  • /bebop1/position_controller_node/bm: 0.016
  • /bebop1/position_controller_node/csvFilesStoring: False
  • /bebop1/position_controller_node/csvFilesStoringTime: 15.0
  • /bebop1/position_controller_node/dev_vx: 0.01
  • /bebop1/position_controller_node/dev_vy: 0.01
  • /bebop1/position_controller_node/dev_vz: 0.01
  • /bebop1/position_controller_node/dev_x: 0.01
  • /bebop1/position_controller_node/dev_y: 0.01
  • /bebop1/position_controller_node/dev_z: 0.01
  • /bebop1/position_controller_node/inertia/xx: 0.00389
  • /bebop1/position_controller_node/inertia/xy: 0.0
  • /bebop1/position_controller_node/inertia/xz: 0.0
  • /bebop1/position_controller_node/inertia/yy: 0.00389
  • /bebop1/position_controller_node/inertia/yz: 0.0
  • /bebop1/position_controller_node/inertia/zz: 0.0078
  • /bebop1/position_controller_node/l: 0.12905
  • /bebop1/position_controller_node/mass: 0.5
  • /bebop1/position_controller_node/mu_phi/mu_phi: 0.0544
  • /bebop1/position_controller_node/mu_psi/mu_psi: 0.44
  • /bebop1/position_controller_node/mu_theta/mu_theta: 0.0543
  • /bebop1/position_controller_node/mu_xy/mu_x: 1
  • /bebop1/position_controller_node/mu_xy/mu_y: 1
  • /bebop1/position_controller_node/mu_z/mu_z: 1
  • /bebop1/position_controller_node/use_sim_time: True
  • /bebop1/position_controller_node/user_account: giuseppe
  • /bebop1/position_controller_node/waypoint_filter: True
  • /bebop1/robot_description: <?xml version="1....
  • /bebop1/tf_prefix: bebop
  • /gazebo/enable_ros_network: True
  • /rosdistro: melodic
  • /rosversion: 1.14.5
  • /use_sim_time: True

NODES
/
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
/bebop1/
position_controller_node (bebop_simulator/position_controller_node)
spawn_robot (gazebo_ros/spawn_model)

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

setting /run_id to e7e5161a-98c8-11ea-a995-a434d9bf2575
process[rosout-1]: started with pid [1381]
started core service [/rosout]
process[gazebo-2]: started with pid [1388]
process[gazebo_gui-3]: started with pid [1393]
process[bebop1/spawn_robot-4]: started with pid [1400]
process[bebop1/position_controller_node-5]: started with pid [1402]
[ INFO] [1589779925.069068160]: Started position controller
[ INFO] [1589779925.079417307]: Got param 'user_account': giuseppe
[ INFO] [1589779925.079807830]: Got param 'waypoint_filter': 1
[ INFO] [1589779925.080103518]: Got param 'csvFilesStoring': 0
[ INFO] [1589779925.080371880]: Got param 'EKFActive': 0
[ INFO] [1589779925.080641432]: Got param 'csvFilesStoringTime': 15.000000
[ INFO] [1589779925.371537649]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1589779925.372710230]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1589779925.407798234]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1589779925.409181029]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[INFO] [1589779925.757121, 0.000000]: Loading model XML from ros parameter robot_description
[INFO] [1589779925.763906, 0.000000]: Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1589779926.780449869]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1589779926.796975908]: Physics dynamic reconfigure ready.
[INFO] [1589779926.969839, 0.151000]: Calling service /gazebo/spawn_urdf_model
[INFO] [1589779927.274034, 0.337000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1589779927.455731321, 0.337000000]: Camera Plugin: Using the 'robotNamespace' param: '/bebop1/'
[ INFO] [1589779927.458800891, 0.337000000]: Camera Plugin (ns = /bebop1/) <tf_prefix_>, set to "bebop"
[bebop1/spawn_robot-4] process has finished cleanly
log file: /home/sheldon/.ros/log/e7e5161a-98c8-11ea-a995-a434d9bf2575/bebop1-spawn_robot-4*.log
[ERROR] [1589779927.541123113, 0.338000000]: Tried to advertise on topic [/bebop1/motor_speed] with md5sum [73fcbf46b49191e672908e50842a83d4] and datatype [std_msgs/Float32], but the topic is already advertised as md5sum [25741daf38ed25442e3a66a855ee8d9c] and datatype [mav_msgs/Actuators]
[FATAL] [1589779927.548930999, 0.338000000]: ASSERTION FAILED
file = /opt/ros/melodic/include/ros/publisher.h
line = 103
cond = false
message =
[FATAL] [1589779927.549018027, 0.338000000]: Call to publish() on an invalid Publisher
[FATAL] [1589779927.549036353, 0.338000000]:

Trace/breakpoint trap (core dumped)
[gazebo-2] process has died [pid 1388, exit code 133, cmd /opt/ros/melodic/lib/gazebo_ros/gzserver -e ode /home/sheldon/catkin_ws/src/BebopS/worlds/basic.world __name:=gazebo __log:=/home/sheldon/.ros/log/e7e5161a-98c8-11ea-a995-a434d9bf2575/gazebo-2.log].
log file: /home/sheldon/.ros/log/e7e5161a-98c8-11ea-a995-a434d9bf2575/gazebo-2*.log
^C[bebop1/position_controller_node-5] killing on exit
[gazebo_gui-3] killing on exit
[gazebo_gui-3] escalating to SIGTERM
`

And this is my launch file:
`

<env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find bebop_simulator)/models"/>
<env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find bebop_simulator)/models"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
   <arg name="world_name" value="$(find bebop_simulator)/worlds/$(arg world_name).world" />
</include>



<!-- Launch UAV -->
<group ns = "$(arg mav_name)1">
<include file="$(find bebop_simulator)/launch/spawn_bebop.launch">
   <arg name="mav_name" value="$(arg mav_name)" />
   <arg name="namespace" value="$(arg mav_name)1"/>
   <arg name="x" value="$(arg x)"/>
   <arg name="y" value="$(arg y)"/>
   <arg name="z" value="$(arg z)"/>
   <arg name="wind_force" value="$(arg wind_force)"/>
   <arg name="wind_start" value="$(arg wind_start)"/>
   <arg name="wind_duration" value="$(arg wind_duration)"/>
   <arg name="wind_direction_x" value="$(arg wind_direction_x)"/>
   <arg name="wind_direction_y" value="$(arg wind_direction_y)"/>
   <arg name="wind_direction_z" value="$(arg wind_direction_z)"/>
   <arg name="enable_imu" value="$(arg enable_imu)"/>
</include>

<!-- Launch the controller  -->
<node name="position_controller_node" pkg="bebop_simulator" type="position_controller_node" output="screen"> 
  <rosparam command="load" file="$(find bebop_simulator)/resource/controller_bebop.yaml" />
  <rosparam command="load" file="$(find bebop_simulator)/resource/bebop.yaml" />
  <rosparam command="load" file="$(find bebop_simulator)/resource/EKF_matrix.yaml" />
  <rosparam command="load" file="$(find bebop_simulator)/resource/waypoint_filter.yaml" />
  <param name="use_sim_time" value="$(arg use_sim_time)" />
  <param name="csvFilesStoring" value="$(arg csvFilesStoring)"/>
  <param name="csvFilesStoringTime" value="$(arg csvFilesStoringTime)"/>
  <param name="user_account" value="$(arg user_account)"/>
  <param name="waypoint_filter" value="$(arg waypoint_filter)"/>
  <param name="EKFActive" value="$(arg EKFActive)"/>
  <!-- <remap from="/command/motor_speed" to="/gazebo/command/motor_speed" /> -->
  <remap from="/odometry" to="/bebop1/odometry" />
  <remap from="/odometry_gt" to="/bebop/odometry_gt" />
  <remap from="/referenceAngles" to="/bebop/referenceAngles" />
  <remap from="/filteredOutput" to="/bebop/filteredOutput" />
  <remap from="/stateErrors" to="/bebop/stateErrors" />
  <remap from="/smoothedTrajectory" to="/bebop/smoothedTrajectory" />
  <remap from="/command/trajectory" to="/$(arg mav_name)1/command/trajectory" />
</node>
</group>

`

gsilano commented

Hi @sheldonjinqi! Have you tried to make a launch file as the one at this link?

sheldonjinqi commented

Hi @gsilano, I used crazyflie2_swarm_hovering_example.launch as a reference when I wrote my launch file. I used the group tag for each robot with a specific namespace. I believe i have found the problem. Without the group tag, there are two topics when running the launch file, /motor_speed and /bebop1/motor_speed. After putting everything in the group tag with namespace bebop_1, the first topic would also have a name /bebop_1/motor_speed and leads to the name conflict.

gsilano commented

Ok, awesome. If you have discovered the issue and now everything works. Could you provide the swarm's example through a PR?

sheldonjinqi commented

Definitely, I would do it later. Another general question regarding the controller. With the provided position controller, when running task2_world, the robot moves slowly and stops at each waypoint. Any suggestion for designing a smoother trajectory such that the robot flies continuously without stopping at each waypoint. I am pretty new to this field, so any suggestion would be great.
Thanks in advance for your help :)

gsilano commented

A possibility could be to replace the waypoint filter (for more details see the reference paper) with a spline trajectory generator. I've already started developing it, but I don't have time to finalize the code. You could start with this.

sheldonjinqi commented

thanks! i will take a look at the paper and try it myself.