Skip to content

Commit efe6a49

Browse files
a-daksha-daksh
authored
Add files via upload
1 parent 33aa3d1 commit efe6a49

4 files changed

+238
-0
lines changed

‎avoidance.py

+95
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
#! /usr/bin/env python
2+
3+
import rospy
4+
5+
from sensor_msgs.msg import LaserScan
6+
from geometry_msgs.msg import Twist
7+
8+
pub = None
9+
thresh=0.25
10+
11+
def clbk_laser(msg):
12+
regions = {
13+
'1':round(min(msg.ranges[0:44]),3),
14+
'2':round(min(msg.ranges[45:89]),3),
15+
'3':round(min(msg.ranges[90:134]),3),
16+
'4':round(min(msg.ranges[135:179]),3),
17+
'5':round(min(msg.ranges[180:224]),3),
18+
'6':round(min(msg.ranges[225:269]),3),
19+
'7':round(min(msg.ranges[270:314]),3),
20+
'8':round(min(msg.ranges[315:360]),3),
21+
}
22+
take_action(regions)
23+
24+
def take_action(regions):
25+
msg = Twist()
26+
linear_x = 0
27+
angular_z = 0
28+
29+
state_description = ''
30+
31+
if regions['1'] > thresh and regions['2'] > thresh and regions['7'] > thresh and regions['8'] > thresh:
32+
state_description = 'case 1 - nothing'
33+
linear_x = 0.2
34+
angular_z = 0
35+
elif regions['1'] < thresh and regions['2'] > thresh and regions['7'] > thresh and regions['8'] < thresh:
36+
state_description = 'case 2 - front'
37+
linear_x = 0
38+
angular_z = -0.15
39+
elif regions['1'] < thresh and regions['7'] > thresh and regions['2'] < thresh and regions['8'] < thresh:
40+
state_description = 'case 6 - front and fleft'
41+
linear_x = 0
42+
angular_z = -0.15
43+
elif regions['8'] > thresh and regions['7'] > thresh and regions['2'] < thresh and regions['1'] < thresh:
44+
state_description = 'case 4 - fleft'
45+
linear_x = 0
46+
angular_z = -0.15
47+
elif regions['1'] > thresh and regions['3'] < thresh and regions['2'] < thresh and regions['8'] > thresh:
48+
state_description = 'case 2 - left'
49+
linear_x = 0.12
50+
angular_z = 0.315
51+
elif regions['1'] > thresh and regions['3'] < thresh and regions['4'] < thresh and regions['8'] > thresh:
52+
state_description = 'case 2 - back left'
53+
linear_x = 0
54+
angular_z = 0.15
55+
elif regions['8'] < thresh and regions['7'] < thresh and regions['2'] > thresh and regions['1'] > thresh:
56+
state_description = 'case 3 - fright'
57+
linear_x = 0
58+
angular_z = -0.15
59+
elif regions['1'] < thresh and regions['7'] < thresh and regions['2'] > thresh and regions['8'] < thresh:
60+
state_description = 'case 5 - front and fright'
61+
linear_x = 0
62+
angular_z = -0.15
63+
elif regions['1'] < thresh and regions['2'] < thresh and regions['7'] < thresh and regions['8'] < thresh:
64+
state_description = 'case 7 - front and fleft and fright'
65+
linear_x = 0
66+
angular_z = -0.15
67+
elif regions['1'] > thresh and regions['2'] < thresh and regions['7'] < thresh and regions['8'] > thresh:
68+
state_description = 'case 8 - fleft and fright'
69+
linear_x = 0
70+
angular_z = -0.15
71+
else:
72+
state_description = 'unknown case'
73+
linear_x=0.6
74+
angular_z=0
75+
rospy.loginfo(regions)
76+
77+
rospy.loginfo(state_description)
78+
msg.linear.x = linear_x
79+
msg.angular.z = angular_z
80+
pub.publish(msg)
81+
82+
83+
def main():
84+
global pub
85+
86+
rospy.init_node('reading_laser')
87+
88+
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
89+
90+
sub = rospy.Subscriber('/scan', LaserScan, clbk_laser)
91+
92+
rospy.spin()
93+
94+
if __name__ == '__main__':
95+
main()

‎reading_laser.py

+27
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,27 @@
1+
#! /usr/bin/env python
2+
import rospy
3+
from sensor_msgs.msg import LaserScan
4+
5+
#360 points, 360/8=45
6+
def clbk_Laser(msg):
7+
regions=[
8+
round(min(msg.ranges[0:44]),3),
9+
round(min(msg.ranges[45:89]),3),
10+
round(min(msg.ranges[90:134]),3),
11+
round(min(msg.ranges[135:179]),3),
12+
round(min(msg.ranges[180:224]),3),
13+
round(min(msg.ranges[225:269]),3),
14+
round(min(msg.ranges[270:314]),3),
15+
round(min(msg.ranges[315:360]),3),
16+
]
17+
rospy.loginfo(regions)
18+
19+
def main():
20+
rospy.init_node('reading_laser')
21+
sub=rospy.Subscriber('/scan',LaserScan,clbk_Laser)
22+
print("1")
23+
rospy.spin()
24+
25+
26+
if __name__=='__main__':
27+
main()

‎turtlebot3_stage_custom.launch

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<launch>
2+
<arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
3+
<arg name="x_pos" default="-0.7"/>
4+
<arg name="y_pos" default="0.0"/>
5+
<arg name="z_pos" default="0.0"/>
6+
7+
<include file="$(find gazebo_ros)/launch/empty_world.launch">
8+
<arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_stage_custom.world"/>
9+
<arg name="paused" value="false"/>
10+
<arg name="use_sim_time" value="true"/>
11+
<arg name="gui" value="true"/>
12+
<arg name="headless" value="false"/>
13+
<arg name="debug" value="false"/>
14+
</include>
15+
16+
17+
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />
18+
19+
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3_burger -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" />
20+
21+
</launch>

‎turtlebot3_stage_custom.world

+95
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
<sdf version='1.4'>
2+
<world name='default'>
3+
<!-- A global light source -->
4+
<include>
5+
<uri>model://sun</uri>
6+
</include>
7+
8+
<!-- A ground plane -->
9+
<include>
10+
<uri>model://ground_plane</uri>
11+
</include>
12+
13+
<physics type="ode">
14+
<real_time_update_rate>1000.0</real_time_update_rate>
15+
<max_step_size>0.001</max_step_size>
16+
<real_time_factor>1</real_time_factor>
17+
<ode>
18+
<solver>
19+
<type>quick</type>
20+
<iters>150</iters>
21+
<precon_iters>0</precon_iters>
22+
<sor>1.400000</sor>
23+
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
24+
</solver>
25+
<constraints>
26+
<cfm>0.00001</cfm>
27+
<erp>0.2</erp>
28+
<contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
29+
<contact_surface_layer>0.01000</contact_surface_layer>
30+
</constraints>
31+
</ode>
32+
</physics>
33+
34+
<!-- Load model -->
35+
<include>
36+
<uri>model://turtlebot3_square</uri>
37+
</include>
38+
39+
<model name='obstacle'>
40+
<pose frame=''>0 0 0 0 -0 0</pose>
41+
<link name='obstacle'>
42+
<collision name='obstacle_1'>
43+
<pose>0 0 0.25 0 0 0</pose>
44+
<geometry>
45+
<cylinder>
46+
<radius>0.15</radius>
47+
<length>0.5</length>
48+
</cylinder>
49+
</geometry>
50+
<max_contacts>10</max_contacts>
51+
<surface>
52+
<bounce/>
53+
<friction>
54+
<ode/>
55+
</friction>
56+
<contact>
57+
<ode/>
58+
</contact>
59+
</surface>
60+
</collision>
61+
62+
<visual name='obstacle_1'>
63+
<pose>0 0 0.25 0 0 0</pose>
64+
<geometry>
65+
<cylinder>
66+
<radius>0.15</radius>
67+
<length>0.5</length>
68+
</cylinder>
69+
</geometry>
70+
<material>
71+
<script>
72+
<uri>file://media/materials/scripts/gazebo.material</uri>
73+
<name>Gazebo/White</name>
74+
</script>
75+
</material>
76+
</visual>
77+
78+
</link>
79+
<static>1</static>
80+
</model>
81+
82+
<scene>
83+
<ambient>0.4 0.4 0.4 1</ambient>
84+
<background>0.7 0.7 0.7 1</background>
85+
<shadows>true</shadows>
86+
</scene>
87+
88+
<gui fullscreen='0'>
89+
<camera name='user_camera'>
90+
<pose>0.0 0.0 17.0 0 1.5708 0</pose>
91+
<view_controller>orbit</view_controller>
92+
</camera>
93+
</gui>
94+
</world>
95+
</sdf>

0 commit comments

Comments
 (0)