search

USC-ACTLab / crazyswarm

A Large Quadcopter Swarm
MIT License
326 stars 319 forks source link

Motive 2.0 with VRPN streaming #121

Closed gy2256 closed 5 years ago

gy2256 commented 5 years ago

Hello,

The current setup in my lab is using Motive 2.0 (NatNet 3.0) with VRPN streaming. Is crazyswam compatible with this setting at this moment?

gy2256 commented 5 years ago

After checking a few previous issues, it looks like the motion capture tracking option uses NatNetLinux as its backend, which is out of support (The latest update was 4 years ago). Is it possible to utilize vrpn_client_ros for Optitrack?

glennliu commented 5 years ago

I think there are two problems,

  1. VRPN doesn't look like offered the interface for getPointCloud. It can work with motionCapture. But I think it won't work with libobjecttracker.
  2. VRPN cannot run with NatNet together, or they would conflict and cause program shut down. So, every function in crazyswarm_server used optitrack.h need to be deleted and rewrite to work with VRPN.

I think Optitrack NatNet is a common problems. We have discussed at #115 as well

whoenig commented 5 years ago

crazyswarm_server doesn't directly rely on optitrack (and only on motionCapture and libobjecttracker). I think its fairly easy to add VRPN support, but as glennliu pointed out, it wont work for libobjecttracker.

glennliu commented 5 years ago

I looked at NatNet from Optitrack. It looks like those lib are mainly for windows client. There isn't even makefile in its lib. So I don't know how to use it with ROS. That's probably why crazyswarm author use NatNetLinux lib rather than original NatNet.

I think VRPN and mocap_optitrack are similar and both would be easy to add. But mocap_optitrack(https://github.com/ros-drivers/mocap_optitrack) has interface for pointcloud and mocap_optitrack author keep updating it, while VRPN hasn't been updated for one year.

So, I would try with mocap_optitrack at the moment.

gy2256 commented 5 years ago

@glennliu Using mocap_optitrack makes a lot of sense to me. I contacted the author of NatNetLinux yesterday and he told me he was no longer involved in Optitrack project since 2014. Therefore, I don't think it will be updated in the future anymore.

I will also look into mocap_optitrack and hopefully we can get it working soon.

glennliu commented 5 years ago

@paradox56 mocap_optitrack is updating to work with Motive 2.0. And I tested mocap_optitrack today and updated my progress at #122 . It would be great if we can work together to solve it.

gy2256 commented 5 years ago

@glennliu Sounds good. I am not in Lab today but I can give your code a test tomorrow. Shall we get motionCapture work first? It looks like you get some error with publishRigidBody in #122

glennliu commented 5 years ago

Yep. I am trying to get motionCapture work first. And publishRigidBody doesn't work correct.

whoenig commented 5 years ago

I just pushed a change which adds VRPN support and tested it with a Vicon system (which, like Optitrack, has VRPN built-in). Its not a fully polished solution, but it should be good enough for trouble shooting etc. Please let me know if that helps.

gy2256 commented 5 years ago

Thanks! I will test out the code today.

gy2256 commented 5 years ago

Here my current OptiTrack setup: Motive 2.1.0 Local Interface (VRPN): 192.168.1.103 Transmission Type: Multicast Rigid Bodies: On Up Axis: Y Up

Here is my hover_swarm.launch parameters:

  firmware: "crazyswarm" # one of "crazyswarm", "bitcraze"
      broadcast_address: "FFE7E7E7E701"
      world_frame: "/world"
      genericLogTopics: ["log1"]
      genericLogTopicFrequencies: [10]
      # genericLogTopic_log1_Variables: ["ekfprof.usec_setup", "ekfprof.usec_innov", "ekfprof.usec_gain", "ekfprof.usec_corr", "ekfprof.usec_cov"]
      # genericLogTopic_log1_Variables: ["profiling.usec_ekf", "profiling.usec_traj", "profiling.usec_ctrl", "profiling.usec_idle"]
      # genericLogTopic_log1_Variables: ["stabilizer.x", "ctrltarget.x", "vicon.x", "stabilizer.z", "ctrltarget.z", "vicon.z"]
      # genericLogTopic_log1_Variables: ["stateEstimate.roll", "ctrlMel.rolld"]
      genericLogTopic_log1_Variables: ["ctrlStat.edist"]
      # genericLogTopic_log1_Variables: ["stateEstimate.yaw", "ctrltarget.yaw", "stateEstimate.x", "ctrltarget.x"] #, "ctrltarget.x", "stateEstimate.roll", "ctrlMel.rolld"]
      firmwareParams: # for all (independent of type)
        commander:
          enHighLevel: 1
        stabilizer:
          estimator: 2 # 1: complementary, 2: kalman, 3: kalmanUSC (only crazyswarm firmware)
          controller: 2 # 1: PID, 2: mellinger
        ring:
          effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
          solidBlue: 255 # if set to solid color
          solidGreen: 0 # if set to solid color
          solidRed: 0 # if set to solid color
          headlightEnable: 0
        #ekf:
        #  ext_var_xy: 1.5e-7 # 1e-7 # 1.5e-7
        #  ext_var_vel: 2e-4 # 2e-4
        #  ext_var_q: 4.5e-3 # 2e-3 # 4.5e-3
        #  gyro_var_xy: 0.2e-2 # 0.2e-4
        #  gyro_var_z: 0.2e-2 # 0.2e-4
        #  acc_var_xyz: 2.4e-3 # 2.4e-3
      # tracking
      motion_capture_type: "vrpn" # one of none,vicon,optitrack,qualisys,vrpn
      object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
      # vicon_host_name: "vicon"
      # optitrack_local_ip: "localhost"
      # optitrack_server_ip: "optitrack"
      # qualisys_host_name: "10.0.5.219"
      # qualisys_base_port: 22222
      vrpn_host_name: "192.168.1.103"
      save_point_clouds: ~/pointCloud.ot
      print_latency: False
      write_csvs: False
      force_no_cache: False
      enable_parameters: True
      enable_logging: False

First, I tried running roslaunch vrpn_client_ros sample.launch server:=192.168.1.103 to test out the connection and it returned me the correct position information:

header: 
  seq: 123
  stamp: 
    secs: 1544283032
    nsecs: 658742807
  frame_id: "world"
pose: 
  position: 
    x: 0.455445706844
    y: 0.0474857166409
    z: 0.557419538498
  orientation: 
    x: 0.00460369791836
    y: -0.0138094127178
    z: -0.00308112404309
    w: -0.999889373779

Next, I tried to run roslaunch crazyswarm hover_swarm.launch alone and got the following error messages:

ROS_MASTER_URI=http://localhost:11311

process[crazyswarm_server-1]: started with pid [28148]
process[joy-2]: started with pid [28149]
process[crazyswarm_teleop-3]: started with pid [28150]
process[rviz-4]: started with pid [28159]
[ INFO] [1544285963.975527018]: Wait for services...
[ INFO] [1544285963.978106913]: Manager ready.
ch: 1
[ INFO] [1544285964.013044993]: Opened joystick: /dev/input/js0. deadzone_: 0.050000.
[ INFO] [1544285964.075385923]: Adding CF: cf1 (radio://0/80/2M/E7E7E7E701, cf1)...
[ INFO] [1544285964.078890486]: CF ctor: 0.003434 s
[ INFO] [1544285964.086809929]: [cf1] Requesting parameters...
[ INFO] [1544285964.097634160]: Found variables in cache.
[ INFO] [1544285964.343802722]: [cf1] reqParamTOC: 0.264862 s
[ INFO] [1544285964.343826403]: Requesting memories...
[ INFO] [1544285964.351458317]: Memories: 5
[ INFO] [1544285964.364020871]: [cf1] Ready. Elapsed: 0.285079 s
[ INFO] [1544285964.364077148]: CF run: 0.285196 s
update commander/enHighLevel to 1
update ring/effect to 16
update ring/headlightEnable to 0
update ring/solidBlue to 255
update ring/solidGreen to 0
update ring/solidRed to 0
update stabilizer/controller to 2
update stabilizer/estimator to 2
update ctrlMel/i_range_m_z to 1500
update ctrlMel/i_range_xy to 2
update ctrlMel/i_range_z to 0.4
update ctrlMel/kR_xy to 70000
update ctrlMel/kR_z to 60000
update ctrlMel/kd_omega_rp to 200
update ctrlMel/kd_xy to 0.2
update ctrlMel/kd_z to 0.4
update ctrlMel/ki_m_z to 500
update ctrlMel/ki_xy to 0.05
update ctrlMel/ki_z to 0.05
update ctrlMel/kp_xy to 0.4
update ctrlMel/kp_z to 1.25
update ctrlMel/kw_xy to 20000
update ctrlMel/kw_z to 12000
update ctrlMel/mass to 0.032
update ctrlMel/massThrust to 132000
[ INFO] [1544285964.379573412]: [cf1] Update parameters
[ INFO] [1544285964.437795201]: Update params: 0.073670 s
[ INFO] [1544285964.438099423]: Started 1 threads
check_vrpn_cookie(): VRPN Note: minor version number doesn't match: (prefer 'vrpn: ver. 07.34', got 'vrpn: ver. 07.33  0').  This is not normally a problem.
[ WARN] [1544285964.438413985]: No updated pose for CF cf1 for 1544285964.438375 s.
[ WARN] [1544285964.438735070]: No updated pose for CF cf1 for 1544285964.438724 s.
tracker: cf1
[ WARN] [1544285964.443068547]: No updated pose for CF cf1 for 1544285964.443055 s.
[ WARN] [1544285964.459731790]: No updated pose for CF cf1 for 1544285964.459706 s.
[ WARN] [1544285964.476771636]: No updated pose for CF cf1 for 1544285964.476748 s.
[ WARN] [1544285964.493138162]: No updated pose for CF cf1 for 1544285964.493122 s.

and nothing is shown is RVIZ.

gy2256 commented 5 years ago

Ok. I made a mistake in setting, it should be object_tracking_type: "motionCapture" and the error is gone.

However, the drone took off and hit the wall when I used the niceHover.py script. What is the coordinate system in Vicon? I believe it is relate to the coordinate transform. I am going to try Z Up in OptiTrack setting next to see if it solves the problem.

whoenig commented 5 years ago

It should match the ROS coordinate system: positive Z = top of CF, positive X = front of CF, positive Y = left of CF. Where front of the CF is where the radio antenna is.

gy2256 commented 5 years ago

@whoenig Got it. I will adjust it accordingly. Do I also need to update to the crazyswarm firmware? I'm currently running the latest bitcraze firmware.

whoenig commented 5 years ago

The latest bitcraze firmware is fine. However, make sure you configure hover_swarm.launch accordingly: It needs to have "bitcraze" here: https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/launch/hover_swarm.launch#L10, and "2" here: https://github.com/USC-ACTLab/crazyswarm/blob/master/ros_ws/src/crazyswarm/launch/hover_swarm.launch#L25.

gy2256 commented 5 years ago

Thanks! It works now.

gy2256 commented 5 years ago

@whoenig Something really strange happened today. I tried to redo the test with everything unchanged and I was unable to get \tf data for the drone. Nothing was displayed in RVIZ and the drone flipped up-side down during take off.

In addition, crazy swarm server is expecting some rigid bodies that is not even created/published in Optitrack. (Rigidbody 2, cf2 ,etc. which actually do not exist)To double check if everything was correct, I launched a separate VRPN ROS node and subscribed to it. Everything looked fine as well.

whoenig commented 5 years ago

crazyswarm_server expects rigid bodies of the form "cf" (for whatever CFs are currently enabled). It uses libmotioncapture, which tries to connect to all available rigid bodies over VRPN.

Can you test the behavior when running roslaunch crazyswarm mocap_helper.launch?

gy2256 commented 5 years ago

@whoenig Sorry about the delay. I was working on some other projects for the past week. Here is my mocap_help.launch setup:

<?xml version="1.0"?>
<launch>

  <node pkg="crazyswarm" type="mocap_helper" name="mocap_helper" output="screen" >
    <rosparam>
      # tracking
      motion_capture_type: "vrpn" # one of vicon,optitrack,qualisys,vrpn
      object_tracking_type: "motionCapture" # one of motionCapture,libobjecttracker
      # vicon_host_name: "vicon"
      # optitrack_local_ip: "localhost"
      # optitrack_server_ip: "optitrack"
      # qualisys_host_name: "10.0.5.219"
      # qualisys_base_port: 22222
      vrpn_host_name: "192.168.1.103"
    </rosparam>
  </node>
</launch>

After running roslaunch crazyswarm mocap_helper.launch, Here is the output:

    points:
    objects:
frame 244:
    points:
    objects:
frame 245:
    points:
    objects:
frame 246:
    points:
    objects:
frame 247:
    points:
    objects:
frame 248:
    points:
    objects:
frame 249:
    points:
    objects:
frame 250:
    points:
    objects:
frame 251:
    points:
    objects:
frame 252:
    points:
    objects:
frame 253:
    points:
    objects:
frame 254:
tracker: create0
tracker: create1
tracker: create2
tracker: marbles
tracker: cf1

"cf1" is the rigidbody that I defined for crazyflie.

Here is the rqt_graph when I launch hover_launch rqt

Here is the video when the crazyflie was functional before: https://vimeo.com/307574915

bconvens commented 5 years ago

Hi, I had a working crazyswarm project for some time, today I recloned and downloaded the default version of the original crazyswarm code again. Unfortunately I now get this build error. Does anyone know how to resolve this? Since it has to do something with VRPN I put my questions in this open issue. Thanks for the help! image image

whoenig commented 5 years ago

You will need to install the vrpn library: sudo apt install ros-kinetic-vrpn.

gy2256 commented 5 years ago

@whoenig Do I need to run a separate VRPN node to get the position working correctly? When I run the ros-kinetic-vrpn, I do get tf information in rviz.

whoenig commented 5 years ago

No, you just need that library installed for the compilation to succeed. The crazyswarm-server will connect to your mocap internally without relying on any third-party node.

gy2256 commented 5 years ago

@whoenig Thank you. I have managed to get the drone flying again. It looks like i need to run pc_permissions.sh before running the launch file.