gsilano / CrazyS

CrazyS is an extension of the ROS package RotorS, aimed to modeling, developing and integrating the Crazyflie 2.0
Apache License 2.0
161 stars 84 forks source link

Not loading world file from RotorS package #57

Closed aka-LJ closed 4 years ago

aka-LJ commented 4 years ago

Hello,

I am on Melodic + 18.04.

I try to change the world file for the MAV hovering example, but I can only load the basic world and Yosemite one.

could you please suggest to me what adaption do I need to load the outdoor world.

welcome[bot] commented 4 years ago

Thanks for opening your first issue here! Make sure that no other issues on the same topic have already been opened!

aka-LJ commented 4 years ago

Here is my world file.

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="outdoor">
    <!-- Place user camera -->
    <gui>
      <camera name="user_camera">
        <pose>-11.660200 -5.942800 6.053900 0.000000 0.443643 0.352194</pose>
      </camera>
    </gui>
    <scene>
      <ambient>0.2 0.2 0.2 1.0</ambient>
      <grid>false</grid>
      <sky>
        <clouds>
          <speed>4</speed>
        </clouds>
      </sky>
    </scene>

    <!-- Only one ROS interface plugin is required per world, as any other plugin can connect a Gazebo
         topic to a ROS topic (or vise versa). -->
    <plugin name="ros_interface_plugin" filename="librotors_gazebo_ros_interface_plugin.so"/>

    <spherical_coordinates>
      <surface_model>EARTH_WGS84</surface_model>
      <latitude_deg>47.3667</latitude_deg>
      <longitude_deg>8.5500</longitude_deg>
      <elevation>500.0</elevation>
      <heading_deg>0</heading_deg>
    </spherical_coordinates>
    <physics type='ode'>
      <ode>
        <solver>
          <type>quick</type>
          <iters>1000</iters>
          <sor>1.3</sor>
        </solver>
        <constraints>
          <cfm>0</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>100</contact_max_correcting_vel>
          <contact_surface_layer>0.001</contact_surface_layer>
        </constraints>
      </ode>
      <max_step_size>0.01</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>100</real_time_update_rate>
      <gravity>0 0 -9.8</gravity>
    </physics>
    <!-- Light Source -->
    <light type="directional" name="sun">
      <cast_shadows>false</cast_shadows>
      <pose>0 0 100 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.9 0.9 0.9 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>0.2 0.2 -0.9</direction>
    </light>
    <light type="point" name="light_bulb">
      <cast_shadows>false</cast_shadows>
      <pose>-14 0 5 0 0 0</pose>
      <diffuse>0.3 0.3 0.3 1</diffuse>
      <specular>0.0 0.0 0.0 1</specular>
      <attenuation>
        <range>80</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
    </light>
    <include>
      <uri>model://vrc_heightmap_1</uri>
    </include>
    <include>
      <uri>model://grass_plane</uri>
    </include>
    <include>
      <uri>model://speed_limit_sign</uri>
      <name>speed_sign_1</name>
      <static>true</static>
      <pose>15 -5 0 0 0 1.5707</pose>
    </include>
    <include>
      <uri>model://speed_limit_sign</uri>
      <name>speed_sign_2</name>
      <static>true</static>
      <pose>70 5 0 0 0 1.5707</pose>
    </include>
    <include>
      <uri>model://speed_limit_sign</uri>
      <name>speed_sign_3</name>
      <static>true</static>
      <pose>121 13 0 0 0 2.356</pose>
    </include>
    <include>
      <uri>model://speed_limit_sign</uri>
      <name>speed_sign_4</name>
      <static>true</static>
      <pose>155 68 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://speed_limit_sign</uri>
      <name>speed_sign_5</name>
      <static>true</static>
      <pose>175 95 0 0 0 1.5707</pose>
    </include>
    <include>
      <uri>model://house_1</uri>
      <name>house_1_a</name>
      <pose>12 9.5 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://house_1</uri>
      <name>house_1_b</name>
      <pose>129.5 46 0 0 0 1.5707</pose>
    </include>
    <include>
      <uri>model://mailbox</uri>
      <name>mailbox_0</name>
      <static>true</static>
      <pose>18 4 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://mailbox</uri>
      <name>mailbox_1</name>
      <static>true</static>
      <pose>24.5 -4.45 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://gas_station</uri>
      <name>gas_station</name>
      <pose>77 15 0 0 0 -1.5707963267948966</pose>
    </include>
    <include>
      <uri>model://house_2</uri>
      <name>house_2_a</name>
      <pose>30 -8 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://house_2</uri>
      <name>house_2_b</name>
      <pose>158 91 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://house_3</uri>
      <name>house_3_a</name>
      <pose>44 7 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://house_3</uri>
      <name>house_3_b</name>
      <pose>111 26 0 0 0 .78539</pose>
    </include>
    <include>
      <uri>model://house_3</uri>
      <name>house_3_c</name>
      <pose>170 106 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://mailbox</uri>
      <name>mailbox_2</name>
      <static>true</static>
      <pose>59 4.45 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://house_3</uri>
      <name>house_3_d</name>
      <pose>95 7.0 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://dumpster</uri>
      <static>true</static>
      <pose>143 51.5 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://house_2</uri>
      <name>house_2_c</name>
      <pose>135 84.8 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_1</name>
      <static>true</static>
      <pose>24 4 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_2</name>
      <static>true</static>
      <pose>54 4 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_3</name>
      <static>true</static>
      <pose>84 4 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_4</name>
      <static>true</static>
      <pose>14 -4 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_5</name>
      <static>true</static>
      <pose>44 -4 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_6</name>
      <static>true</static>
      <pose>74 -4 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_7</name>
      <static>true</static>
      <pose>148 104 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_8</name>
      <static>true</static>
      <pose>178 104 0 0 0 0</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_9</name>
      <static>true</static>
      <pose>158 96 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_10</name>
      <static>true</static>
      <pose>188 96 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_11</name>
      <static>true</static>
      <pose>102 8 0 0 0 .785</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_12</name>
      <static>true</static>
      <pose>123 29 0 0 0 .785</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_13</name>
      <static>true</static>
      <pose>144 50 0 0 0 .785</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_14</name>
      <static>true</static>
      <pose>99 -4 0 0 0 3.1415</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_15</name>
      <static>true</static>
      <pose>120 14.5 0 0 0 -2.35</pose>
    </include>
    <include>
      <uri>model://lamp_post</uri>
      <name>lamp_post_16</name>
      <static>true</static>
      <pose>141 35.5 0 0 0 -2.35</pose>
    </include>
    <road name="main_street">
      <width>7.34</width>
      <point>0 0 0.01</point>
      <point>100 0 0.01</point>
      <point>150 50 0.01</point>
      <point>150 100 0.01</point>
      <point>200 100 0.01</point>
    </road>
    <road name="gas_station_street">
      <width>17</width>
      <point>65 25 0.01</point>
      <point>65 3.66 0.01</point>
    </road>
    <model name="road_boxes">
      <static>true</static>
      <link name="link">
        <collision name="collision_1">
          <pose>51 0 0 0 0 0</pose>
          <geometry>
            <box>
              <size>102 7.34 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.8</mu>
                <mu2>0.8</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <collision name="collision_2">
          <pose>125 25 0 0 0 0.78539</pose>
          <geometry>
            <box>
              <size>72 7.34 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.8</mu>
                <mu2>0.8</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <collision name="collision_3">
          <pose>150 75 0 0 0 0</pose>
          <geometry>
            <box>
              <size>7.34 53 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.8</mu>
                <mu2>0.8</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <collision name="collision_4">
          <pose>173.5 100 0 0 0 0</pose>
          <geometry>
            <box>
              <size>54.5 7.34 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.8</mu>
                <mu2>0.8</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
        <collision name="collision_5">
          <pose>198 106 0 0 0 0</pose>
          <geometry>
            <box>
              <size>6 5 0.02</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.8</mu>
                <mu2>0.8</mu2>
              </ode>
            </friction>
          </surface>
        </collision>
      </link>
    </model>
  </world>
</sdf>
gsilano commented 4 years ago

Ok. Let's suppose your world file makes sense. What I'm wondering how did you modify the launch file to work with it? Could you copy and past it?

aka-LJ commented 4 years ago

sorry for the late reply, here is the launch file

<launch>
  <arg name="mav_name" default="firefly"/>
  <arg name="world_name" default="outdoor"/>
  <arg name="enable_logging" default="false" />
  <arg name="enable_ground_truth" default="true" />
  <arg name="log_file" default="$(arg mav_name)" />
  <arg name="path" default=""/>
  <arg name="changeIdFrame" default="/talker/changes"/>

  <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
  <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
    <!-- <arg name="debug" value="true"/> -->
    <arg name="paused" value="true"/>
    <!-- <arg name="gui" value="false"/> -->
  </include>

  <group ns="$(arg mav_name)">
    <include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
      <arg name="mav_name" value="$(arg mav_name)" />
      <!-- <arg name="model" value="$(find rotors_description)/urdf/mav_with_vi_AND_wind.gazebo" /> -->
      <arg name="model" value="$(find rotors_description)/urdf/mav_with_vi_sensor.gazebo" />

      <arg name="enable_logging" value="$(arg enable_logging)" />
      <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
      <arg name="log_file" value="$(arg log_file)"/>

    </include>
    <node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
      <rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
      <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
      <remap from="odometry" to="odometry_sensor1/odometry" />
    </node>

    <node name="auto_navi" pkg="rotors_gazebo" type="auto_navi" output="screen"/> 
    <node pkg="rostopic" type="rostopic" name="rostopic_dest" 
          args="pub  -1 /navigation_destination geometry_msgs/Point  -- 30 15 2"/>
    <node pkg="rostopic" type="rostopic" name="rostopic_restricted_area" 
          args="pub  -1 /Bounding_Area rotors_gazebo/restrictedSquare  --  11200 -140 -300  -200"/>

    <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  </group>

  <!-- you can load an exisiting tree with <node ... args="tree.bt"> !-->
  <node pkg="octomap_server" type="octomap_tracking_server_node" name="octomap_talker" output="screen" args="$(arg path)">
    <param name="resolution" value="0.2" />
    <param name="frame_id" type="string" value="world" />
    <param name="sensor_model/max_range" value="10.0" />
    <param name="save_directory" value="$(optenv OCTOMAP_SAVE_DIR ./)" />
    <param name="track_changes" value="true"/>
    <param name="listen_changes" value="false"/>
    <param name="topic_changes" value="/octomap_tracking_server/changeset" />
    <param name="change_id_frame" value="$(arg changeIdFrame)" />
    <param name="min_change_pub" value="0" />
    <remap from="cloud_in" to="/firefly/vi_sensor/camera_depth/depth/points" />
  </node>

</launch>
gsilano commented 4 years ago

Hmm. It is strange to me. Let me understand, are you able to run a simple simulation scenario with Crazyflie? I mean, one of those listed in the README file?

aka-LJ commented 4 years ago

Hmm. It is strange to me. Let me understand, are you able to run a simple simulation scenario with Crazyflie? I mean, one of those listed in the README file?

NO, I tried the mav_hovering_example.launch from rotors package, and the gazebo is not launching.

then I change the world to outdoor in crazyflie2_hovering_example.launch. gazebo is launching, but the world is not loading properly, and the drone is not hovering. stuck in the initial stage

gsilano commented 4 years ago

Ah, ok. So, you also have problems when you try to lunch the Crazyflie example. Am I right?