Skip to content

Commit

Permalink
[modules/rangemodules + gazebo] *fixed pointer problem to range sensors
Browse files Browse the repository at this point in the history 
* fixed unresponsive forcefield on the range sensors
  • Loading branch information
@knmcguire @kirkscheper
knmcguire authored and kirkscheper committed Nov 3, 2017
1 parent f7149da commit 27196d4
Show file tree
Hide file tree
Showing 5 changed files with 31 additions and 28 deletions.
1 change: 1 addition & 0 deletions conf/flight_plans/rotorcraft_rangesensors.xml
View file
Expand Up @@ -57,6 +57,7 @@
<set var="vel_body_x_guided" value="0.0"/>
<set var="vel_body_y_guided" value="0.0"/>
<call_once fun="range_sensor_horizontal_velocity_force_field(&vel_body_x_guided,&vel_body_y_guided, inner_border_FF, outer_border_FF, 0, min_vel_command, max_vel_command)"/>
<call_once fun="guidance_v_set_guided_z(-1.5)"/>
<call_once fun="guidance_h_set_guided_body_vel(vel_body_x_guided,vel_body_y_guided)"/>
<deroute block="Hover with range sensors"/>
</block>
Expand Down
4 changes: 2 additions & 2 deletions conf/simulator/gazebo/models/ardrone/ardrone.sdf
View file
Expand Up @@ -254,8 +254,8 @@
</include>

<joint name="range_sensor_joint" type="fixed">
<parent>chasis</parent>
<child>base</child>
<parent>chassis</parent>
<child>range_sensors::base</child>
</joint>

</model>
Expand Down
12 changes: 6 additions & 6 deletions conf/simulator/gazebo/models/range_sensors/range_sensors.sdf
View file
@@ -1,11 +1,11 @@
<?xml version='1.0'?>
<sdf version='1.4'>
<model name="range_sensors">
<pose>0 0 0 0 0 0</pose>


<link name="base">
<inertial><!-- Taken from Paparazzi's ARDrone model for JSBsim -->
<mass>0.4</mass>
<mass>0.001</mass>
<inertia>
<ixx> 0.005 </ixx>
<iyy> 0.005 </iyy>
Expand Down Expand Up @@ -73,7 +73,7 @@
<ixz>0</ixz>
</inertia>
</inertial>
<sensor type= "ray" name="front_range_sensor">
<sensor type= "ray" name="up_range_sensor">
<visualize>true</visualize>
<update_rate>30</update_rate>
<ray>
Expand All @@ -92,7 +92,7 @@
</joint>

<link name="left_range_sensor">
<pose>0.0 0 0.1 0 0 -1.57</pose>
<pose>0.0 0 0.1 0 0 1.57</pose>
<inertial>
<mass>0.001</mass>
<inertia>
Expand All @@ -118,12 +118,12 @@
</link>

<joint name="left_range_sensor_joint" type="fixed">
<parent>chassis</parent>
<parent>base</parent>
<child>left_range_sensor</child>
</joint>

<link name="right_range_sensor">
<pose>0.0 0 0.1 0 0 1.57</pose>
<pose>0.0 0 0.1 0 0 -1.57</pose>
<inertial>
<mass>0.001</mass>
<inertia>
Expand Down
11 changes: 6 additions & 5 deletions sw/airborne/modules/range_module/range_module.c
View file
Expand Up @@ -47,13 +47,14 @@ struct range_finders_ range_finders;
static void range_sensors_cb(uint8_t UNUSED(sender_id),
int16_t range_front, int16_t range_right, int16_t range_back, int16_t range_left, int16_t range_bottom, int16_t range_top)
{

range_finders.front = (float)range_front/1000;
range_finders.right = (float)range_right/1000;
range_finders.back = (float)range_back/1000;
range_finders.left = (float)range_left/1000;
range_finders.top = (float)range_top/1000;
range_finders.bottom = (float)range_bottom/1000;


}

void range_init(void)
Expand Down Expand Up @@ -111,7 +112,7 @@ void range_sensor_horizontal_velocity_force_field(float *vel_body_x, float *vel_
float avoid_y_command = *vel_body_y;

// Balance avoidance command for y direction (sideways)
if (range_finders.right < 1 || range_finders.right > max_sensor_range) {
if (range_finders.right < 0.001 || range_finders.right > max_sensor_range) {
//do nothing
} else if (range_finders.right < avoid_inner_border) {
avoid_y_command -= max_vel_command_lc;
Expand All @@ -122,7 +123,7 @@ void range_sensor_horizontal_velocity_force_field(float *vel_body_x, float *vel_
/ (float)difference_inner_outer;
} else {}

if (range_finders.left < 1 || range_finders.left > max_sensor_range) {
if (range_finders.left < 0.001 || range_finders.left > max_sensor_range) {
//do nothing
} else if (range_finders.left < avoid_inner_border) {
avoid_y_command += max_vel_command_lc;
Expand All @@ -134,7 +135,7 @@ void range_sensor_horizontal_velocity_force_field(float *vel_body_x, float *vel_
} else {}

// balance avoidance command for x direction (forward/backward)
if (range_finders.front < 1 || range_finders.front > max_sensor_range) {
if (range_finders.front < 0.001 || range_finders.front > max_sensor_range) {
//do nothing
} else if (range_finders.front < avoid_inner_border) {
avoid_x_command -= max_vel_command_lc;
Expand All @@ -146,7 +147,7 @@ void range_sensor_horizontal_velocity_force_field(float *vel_body_x, float *vel_
} else {}


if (range_finders.back < 1 || range_finders.back > max_sensor_range) {
if (range_finders.back < 0.001 || range_finders.back > max_sensor_range) {
//do nothing
} else if (range_finders.back < avoid_inner_border) {
avoid_x_command += max_vel_command_lc;
Expand Down
31 changes: 16 additions & 15 deletions sw/simulator/nps/nps_fdm_gazebo.cpp
View file
Expand Up @@ -689,28 +689,29 @@ static void gazebo_init_range_sensors(void)
{
gazebo::sensors::SensorManager *mgr =
gazebo::sensors::SensorManager::Instance();
cout<<"Amount of sensors found: "<<model->GetSensorCount()<<endl;
gazebo_range_sensors.ray_front = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("front_range_sensor"));
gazebo_range_sensors.ray_right = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("right_range_sensor"));
gazebo_range_sensors.ray_back = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("back_range_sensor"));
gazebo_range_sensors.ray_left = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("left_range_sensor"));
gazebo_range_sensors.ray_up = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("up_range_sensor"));
gazebo_range_sensors.ray_down = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("down_range_sensor"));


if (!gazebo_range_sensors.ray_left||!gazebo_range_sensors.ray_right||!gazebo_range_sensors.ray_up||
cout<<"Amount of sensors found: "<<model->GetSensorCount()<<endl;
gazebo_range_sensors.ray_front = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("range_sensors::front_range_sensor"));
gazebo_range_sensors.ray_right = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("range_sensors::right_range_sensor"));
gazebo_range_sensors.ray_back = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("range_sensors::back_range_sensor"));
gazebo_range_sensors.ray_left = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("range_sensors::left_range_sensor"));
gazebo_range_sensors.ray_up = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("range_sensors::up_range_sensor"));
gazebo_range_sensors.ray_down = static_pointer_cast<gazebo::sensors::RaySensor>(mgr->GetSensor("range_sensors::down_range_sensor"));

if (!gazebo_range_sensors.ray_left||!gazebo_range_sensors.ray_right||!gazebo_range_sensors.ray_up||
!gazebo_range_sensors.ray_down||!gazebo_range_sensors.ray_front||!gazebo_range_sensors.ray_back) {
cout << "ERROR: Could not get pointer to raysensor!" << gazebo_range_sensors.ray_left<<gazebo_range_sensors.ray_right<<gazebo_range_sensors.ray_up<<
gazebo_range_sensors.ray_down<<gazebo_range_sensors.ray_front<<gazebo_range_sensors.ray_back<<endl;
}

gazebo_range_sensors.ray_left->SetActive(true);
gazebo_range_sensors.ray_right->SetActive(true);
gazebo_range_sensors.ray_up->SetActive(true);
gazebo_range_sensors.ray_down->SetActive(true);
gazebo_range_sensors.ray_front->SetActive(true);
gazebo_range_sensors.ray_back->SetActive(true);

gazebo_range_sensors.ray_left->SetActive(true); cout<<"check"<<endl;

gazebo_range_sensors.ray_right->SetActive(true);cout<<"check"<<endl;
gazebo_range_sensors.ray_up->SetActive(true);cout<<"check"<<endl;
gazebo_range_sensors.ray_down->SetActive(true);cout<<"check"<<endl;
gazebo_range_sensors.ray_front->SetActive(true);cout<<"check"<<endl;
gazebo_range_sensors.ray_back->SetActive(true);cout<<"check"<<endl;

}
static void gazebo_read_range_sensors(void)
Expand Down

0 comments on commit 27196d4

Please sign in to comment.