Skip to content

Commit 6e450c5

Browse files
rishabh1brishabh1b
committed
Localization more issues
1 parent f75a566 commit 6e450c5

6 files changed

Lines changed: 48 additions & 70 deletions

File tree

‎src/rbiyani_project_5/adventure_simulator/adventure_description/finn/finn.urdf.xacro‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -10,17 +10,17 @@
1010
<xacro:include filename="$(find adventure_description)/urdf/sensors/asus_xtion_pro.urdf.xacro"/>
1111
<xacro:sensor_asus_xtion_pro parent="base_link" xyz="0.14 -0.02 0.1" rpy="3.14 0 0"/>
1212

13-
<xacro:include filename="$(find turtlebot_arm_description)/urdf/arm.xacro" />
13+
<!--xacro:include filename="$(find turtlebot_arm_description)/urdf/arm.xacro" /-->
1414

1515
<!-- Turtlebot arm macro: we can configure joints velocity limit and lower/upper limits
1616
for the first joint (arm_shoulder_pan) to allow accessing to different operational
1717
areas, e.g. left handed vs. right handed robot -->
18-
<xacro:turtlebot_arm parent="base_link" color="white" gripper_color="green"
19-
joints_vlimit="1.571" pan_llimit="-2.617" pan_ulimit="2.617">
18+
<!--xacro:turtlebot_arm parent="base_link" color="white" gripper_color="green"
19+
joints_vlimit="1.571" pan_llimit="-2.617" pan_ulimit="2.617"-->
2020
<!-- Place the "floating" arm at the location it should be if mounted on a turtlebot,
2121
as pick and place and other demos assume this location -->
22-
<origin xyz="0 0 0.1"/>
23-
</xacro:turtlebot_arm>
22+
<!--origin xyz="0 0 0.1"/-->
23+
<!--/xacro:turtlebot_arm-->
2424

2525

2626
<!-- origin xyz="${0} 0 0" rpy="0.0 0.0 0.0"/-->

‎src/rbiyani_project_5/adventure_simulator/adventure_gazebo/launch/adventure.launch‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
<?xml version="1.0"?>
2+
3+
<launch>
4+
<include file="$(find adventure_gazebo)/launch/adventure_world.launch"/>
5+
<node name="laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" respawn="true">
6+
<remap from="image" to="/camera/depth/image_raw"/>
7+
</node>
8+
<include file="$(find adventure_teleop)/launch/adventure_teleop.launch"/>
9+
<include file="$(find adventure_slam)/launch/adventure_slam.launch"/>
10+
11+
</launch>

‎src/rbiyani_project_5/adventure_simulator/adventure_gazebo/launch/adventure_world.launch‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<launch>
44
<!-- start Rviz -->
55
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find adventure_gazebo)/launch/adventure.rviz"/>
6-
<arg name="urdf_file" value="$(find xacro)/xacro.py '$(find turtlebot_arm_description)/urdf/arm.urdf.xacro'" />
6+
<!--arg name="urdf_file" value="$(find xacro)/xacro.py '$(find turtlebot_arm_description)/urdf/arm.urdf.xacro'" /-->
77

88
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
99
<arg name="paused" default="false"/>
@@ -25,7 +25,7 @@
2525
<!-- Load the URDF into the ROS Parameter Server and spawn it in Gazebo-->
2626
<!-- Spawn the robot -->
2727
<include file="$(find adventure_gazebo)/launch/spawn_finn.launch">
28-
<arg name="x" value="-19"/>
29-
<arg name="y" value="-1"/>
28+
<arg name="x" value="-7"/>
29+
<arg name="y" value="18"/>
3030
</include>
3131
</launch>

‎src/rbiyani_project_5/adventure_slam/launch/adventure_slam.launch‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010

1111

1212
<node pkg="adventure_slam" name="map_maker" type="map_maker_nd">
13-
<param name="use_vo" value="false"/>
13+
<param name="use_vo" value="true"/>
1414
<param name="resolution" value="0.1"/>
1515
<param name="map_size" value="1000"/>
1616

‎src/rbiyani_project_5/adventure_slam/src/Localizer.cpp‎

Lines changed: 27 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -48,21 +48,21 @@ void Localizer::estimate()
4848
{
4949
delta_yaw = delta_yaw - 360;
5050
}
51-
if (std::abs(del_yaw) > 1)
51+
if (std::abs(del_yaw) > 0.5)
5252
return;
5353

5454
int num_pairs = 0;
5555

5656
double curr_shift_x = 0, curr_shift_y = 0;
5757
int i, j, count_x = 0, count_y = 0;
5858
double first_distance, first_angle, second_distance, sec_angle;
59-
count = 0;
60-
/*for (i = 0; i < sz ; i++)
59+
int size_max = std::min(sz,2);
60+
for (i = 0; i < size_max ; i++) //sz limited to 2
6161
{
6262
//Simple Averaging approach
6363
LineMatcher::Pair curr_pair = matched_pairs[i];
6464

65-
if (std::abs(curr_pair.L2.angle - curr_pair.L1.angle) > 35 || std::abs(curr_pair.L2.angle - curr_pair.L1.angle) < 0.01) // False values
65+
if (std::abs(curr_pair.L2.angle - curr_pair.L1.angle) > 35)// || std::abs(curr_pair.L2.angle - curr_pair.L1.angle) < 0.01) // False values
6666
{
6767
if (std::abs(curr_pair.L2.angle - curr_pair.L1.angle) > 35)
6868
{
@@ -76,6 +76,9 @@ void Localizer::estimate()
7676

7777
first_distance = curr_pair.L1.distance - curr_pair.L2.distance;
7878

79+
if (first_distance > 0.5)
80+
continue;
81+
7982
first_angle = (curr_pair.L1.angle + curr_pair.L2.angle) / 2;
8083
ROS_INFO("first_angle :%f", first_angle);
8184
first_angle = first_angle * PI / 180;
@@ -91,72 +94,36 @@ void Localizer::estimate()
9194

9295
ROS_INFO("curr_d_x: %lf", d_x);
9396
ROS_INFO("curr_d_y: %lf", d_y);
94-
}*/
95-
96-
97-
for (int i = 0; i < sz; i++)
98-
{
99-
LineMatcher::Pair curr_pair = matched_pairs[i];
100-
101-
if ((std::abs(curr_pair.L2.angle - curr_pair.L1.angle) > 35) || std::abs(curr_pair.L2.distance - curr_pair.L1.distance) > 10) // False values
102-
{
103-
continue;
104-
}
105-
97+
}
10698

107-
if (sz == 1)
99+
// Following logic was based on Line Intersection - Didn't work out very well
100+
/*if (sz == 1)
108101
{
109-
first_distance = curr_pair.L1.distance - curr_pair.L2.distance;
110-
first_angle = (curr_pair.L1.angle + curr_pair.L2.angle) / 2;
111-
first_angle = first_angle * PI / 180;
112-
curr_shift_x += first_distance * std::cos(first_angle);
113-
curr_shift_y += first_distance * std::sin(first_angle);
114-
break;
102+
curr_shift_x += first_distance * std::cos(first_angle);
103+
curr_shift_y += first_distance * std::sin(first_angle);
104+
count++;
105+
break;
115106
}
116-
117107
for (j = i+1; j < sz; j ++)
118108
{
119109
LineMatcher::Pair sec_pair = matched_pairs[j];
110+
second_distance = sec_pair.L1.distance - sec_pair.L2.distance;
111+
sec_angle = (sec_pair.L1.angle + sec_pair.L2.angle) / 2;
112+
sec_angle = sec_angle * PI / 180;
120113
121-
if ((std::abs(sec_pair.L2.angle - sec_pair.L1.angle) > 35) || std::abs(sec_pair.L2.distance - sec_pair.L1.distance) > 10)// False values
122-
{
123-
continue;
124-
}
125-
126-
if (std::abs(sec_pair.L1.angle - curr_pair.L1.angle) < 10)
127-
continue;
128-
129-
double x_old = getXEstimate(curr_pair.L1.distance, curr_pair.L1.angle * PI / 180, sec_pair.L1.distance, sec_pair.L1.angle * PI / 180);
130-
double x_new = getXEstimate(curr_pair.L2.distance, curr_pair.L2.angle * PI / 180, sec_pair.L2.distance, sec_pair.L2.angle * PI / 180);
131-
double y_old = getYEstimate(curr_pair.L1.distance, curr_pair.L1.angle * PI / 180, sec_pair.L1.distance, sec_pair.L1.angle * PI / 180);
132-
double y_new = getYEstimate(curr_pair.L2.distance, curr_pair.L2.angle * PI / 180, sec_pair.L2.distance, sec_pair.L2.angle * PI / 180);
133-
134-
ROS_INFO("(x_old): %lf", x_old);
135-
ROS_INFO("(x_new): %lf", x_new);
136-
ROS_INFO("(y_old): %lf", y_old);
137-
ROS_INFO("(y_new ): %lf", y_new );
138-
139-
curr_shift_x += (x_old - x_new);
140-
curr_shift_y += (y_old - y_new);
141-
142-
//ROS_INFO("curr_shift_x: %f", curr_shift_x);
143-
//ROS_INFO("curr_shift_y: %f", curr_shift_y);
114+
curr_shift_x += getXEstimate(first_distance, first_angle, second_distance, sec_angle);
115+
curr_shift_y += getYEstimate(first_distance, first_angle, second_distance, sec_angle);
144116
count++;
145117
}
146-
}
118+
}*/
147119

120+
ROS_INFO("count_x: %d", count_x);
121+
ROS_INFO("count_y: %d", count_y);
148122

149-
150-
/*if (count_x != 0)
123+
if (count_x != 0)
151124
curr_shift_x = curr_shift_x / count_x;
152125
if (count_y != 0)
153-
curr_shift_y = curr_shift_y / count_y;*/
154-
155-
if (count != 0)
156-
{
157-
curr_shift_x = curr_shift_x / count;
158-
curr_shift_y = curr_shift_y / count;
159-
}
126+
curr_shift_y = curr_shift_y / count_y;
160127

161128
estimateGlobalPosition(curr_shift_x, curr_shift_y);
162129

@@ -168,14 +135,14 @@ void Localizer::estimateGlobalPosition(double curr_shift_x, double curr_shift_y)
168135
delta_yaw = 0;
169136
double curr_ang = delta_yaw * M_PI / 180;
170137
double glob_curr_shift_x = -(std::cos(curr_ang) * curr_shift_x - std::sin(curr_ang) * curr_shift_y);
171-
double glob_curr_shift_y = (std::sin(curr_ang) * curr_shift_x + std::cos(curr_ang) * curr_shift_y);
138+
double glob_curr_shift_y = std::sin(curr_ang) * curr_shift_x + std::cos(curr_ang) * curr_shift_y;
172139

173140
shift_x += glob_curr_shift_x;
174141
shift_y += glob_curr_shift_y;
175142
}
176143

177144
// Utility Methods for Line Intersection Logic
178-
145+
/*
179146
double Localizer::getXEstimate(double fir_dist, double fir_angle, double sec_dist, double sec_angle)
180147
{
181148
return (sec_dist * std::sin(fir_angle) - fir_dist * std::sin(sec_angle)) / std::sin(fir_angle - sec_angle);
@@ -184,4 +151,4 @@ double Localizer::getXEstimate(double fir_dist, double fir_angle, double sec_dis
184151
double Localizer::getYEstimate(double fir_dist, double fir_angle, double sec_dist, double sec_angle)
185152
{
186153
return (sec_dist * std::cos(fir_angle) - fir_dist * std::cos(sec_angle)) / std::sin(sec_angle - fir_angle);
187-
}
154+
}*/

‎src/rbiyani_project_5/adventure_slam/src/adventure_slam.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -201,7 +201,7 @@ int main(int argc, char* argv[])
201201
ros::init(argc, argv, "adventure_slam");
202202
ros::NodeHandle n;
203203

204-
if (!n.getParam("adventure_slam/turn_on_visualization", turn_on_visualization)) turn_on_visualization = false;
204+
if (!n.getParam("adventure_slam/turn_on_visualization", turn_on_visualization)) turn_on_visualization = true;
205205
if (!n.getParam("adventure_slam/matching_line_threshold", matching_line_threshold)) matching_line_threshold = 10;
206206
if (!n.getParam("adventure_slam/distance_threshold", distance_threshold)) distance_threshold = 0.003;
207207
if (!n.getParam("adventure_slam/minimum_no_inliers", minimum_no_inliers)) minimum_no_inliers = 40;

0 commit comments

Comments
 (0)