Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

working with crazy-swarm server and flow-deck #132

Open
GalBrandwine opened this issue Feb 3, 2019 · 1 comment
Open

working with crazy-swarm server and flow-deck #132

GalBrandwine opened this issue Feb 3, 2019 · 1 comment

Comments

@GalBrandwine
Copy link

@GalBrandwine GalBrandwine commented Feb 3, 2019

Hello all.

We have sucsesfuly flown 3 crazyflies with optitrack mocap.
the hover_swarm.launch, and the figure8_csv.py, worked perfectly.

Now we want to use flow deck:
for that we have changed the necesary changed in the hover_swarm.launch file:

(motion_capture_type: "none")

<?xml version="1.0"?>
<launch>
  <arg name="joy_dev" default="/dev/input/js0" />

  <rosparam command="load" file="$(find crazyswarm)/launch/crazyflieTypes.yaml" />
  <rosparam command="load" file="$(find crazyswarm)/launch/crazyflies.yaml" />

  <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
    <rosparam>
      firmware: "bitcraze" # one of "crazyswarm", "bitcraze"
      broadcast_address: "FFE7E7E7E7"
      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: "none" # one of none,vicon,optitrack,qualisys,vrpn
      object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
      # vicon_host_name: "vicon"
      optitrack_local_ip: "169.254.148.87"
      optitrack_server_ip: "169.254.148.86"
      # qualisys_host_name: "10.0.5.219"
      # qualisys_base_port: 22222
      # vrpn_host_name: "vicon"
      save_point_clouds: ~/pointCloud.ot
      print_latency: False
      write_csvs: False
      force_no_cache: False
      enable_parameters: True
      enable_logging: False
    </rosparam>
  </node>

  <node name="joy" pkg="joy" type="joy_node" output="screen">
    <param name="dev" value="$(arg joy_dev)" />
  </node>

  <node pkg="crazyswarm" type="crazyswarm_teleop" name="crazyswarm_teleop" output="screen">
    <param name="csv_file" value="$(find crazyswarm)/launch/figure8_smooth.csv" />
    <param name="timescale" value="0.8" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find crazyswarm)/launch/test.rviz"/>

  <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" args="/cf2/log1/values[0]"/> -->
  <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_roll" args="/cf1/log1/values[2] /cf1/log1/values[3]"/> -->

</launch>

but when we test it by doing:
roslaunch crazyswarm hover_swarm.launch
and than:
rosservice call /cfX/takeoff...

But then the crazyflie is going crazy, and not taking off in a stable and elegant way.
we use the "log1", to log our stateEstimate.x & stateEstimate.y & stateEstimate.z.
they are publishing the initial location of the drone.
and when we try to manually lift the drone we receive the correct feedback.

what i want to know:
0. why the crazyflie is going crazy?

  1. is there a way to "reset" the kalman on start?
  2. where can i find a list of all available parameters to log with the genericLogTopic_log1_Variables
@whoenig

This comment has been minimized.

Copy link
Contributor

@whoenig whoenig commented Feb 3, 2019

  1. Most likely there is a problem with the state estimate. We did test the figure8_csv example just by using the flow deck and it worked fine for us. Which firmware version are you using?
  2. Yes, you can set some parameters (see https://github.com/whoenig/crazyflie_ros/blob/master/crazyflie_demo/scripts/publish_external_position_vicon.py#L15-L22).
  3. You can run rosrun crazyflie_tools listParams --uri <URI-of-CF> to find a list of all parameters and you can use listLogVariables to find a list of all variables you can log. Unfortunately, there is no documentation on what the variables actually mean (and what the units are), apart from reading the crazyflie firmware code.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Linked pull requests

Successfully merging a pull request may close this issue.

None yet
2 participants
You can’t perform that action at this time.