forked from jsk-ros-pkg/jsk_aerial_robot
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #5 from tongtybj/obstacle_detection
[agile quadrotor] add obstacle world spawner and collision detection
- Loading branch information
Showing
6 changed files
with
297 additions
and
13 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,10 @@ | ||
| rpg_box01, 28.880242874670586, -4.621370071256202, 5.850691814486472, 0.2689966635473574, 0.6413589692474388, -0.08470033522892534, 0.7135301821074987, 0.2, 0.2, 0.2 | ||
| rpg_box01, 13.283472918526883, -1.4789642039518114, 8.542338371468412, 0.26814895645283326, -0.7398582577232313, -0.4661710291015493, -0.4042158671531878, 0.5, 0.5, 0.5 | ||
| rpg_box01, 24.65914015053152, -1.471073769584521, 2.4687020614531696, 0.20742823899014262, 0.8887813249354812, 0.1149513441168945, 0.3922084529954679, 0.3434823307368431, 0.3434823307368431, 0.3434823307368431 | ||
| rpg_box01, 5.297416599080256, -5.193077992699491, 6.646607819851377, 0.8298647232348965, 0.0773420885007401, 0.30956229492540444, -0.4577269142590691, 0.45, 0.45, 0.45 | ||
| rpg_box01, 18.86106459236055, -2.238803009571142, 6.643364433521313, 0.5822432509953802, -0.6935973990501345, -0.259563930105829, -0.3354728169155222, 0.25, 0.25, 0.25 | ||
| rpg_box01, 17.765383636944833, 1.4630835093451182, 1.8282995609825714, 0.06258331613146262, 0.6961375957648155, 0.0825437070361798, 0.7103958845133447, 0.25, 0.25, 0.25 | ||
| rpg_box01, 26.505801640223147, 2.8436730861196295, 7.555012129595632, 0.7147410029244046, -0.6495305407292438, 0.12716012622318898, 0.22602140982105023, 0.25, 0.25, 0.25 | ||
| rpg_box01, 10.005753877588159, 7.706205446229269, 6.337724557521747, 0.5313859532728835, -0.5200789397958456, 0.5578063977798602, 0.3687802701351503, 0.25, 0.25, 0.25 | ||
| rpg_box01, 39.423427193141244, -0.9487384773484564, 6.977949146765134, 0.6148284812321693, -0.2135605486720743, -0.6839238130906621, 0.3295846607537367, 0.25, 0.25, 0.25 | ||
| rpg_box01, 40.499735390524364, -8.62492358974646, 3.8495739887431646, 0.365725076621789, -0.29038137842215483, -0.005483313680931745, 0.8842475652591375, 0.25, 0.25, 0.25 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,145 @@ | ||
| #!/usr/bin/env python | ||
|
|
||
| import rospy | ||
| from gazebo_ros import gazebo_interface | ||
| from gazebo_msgs.srv import SpawnModel, DeleteModel | ||
| from geometry_msgs.msg import Pose, Point, Quaternion | ||
| from nav_msgs.msg import Odometry | ||
| from std_msgs.msg import Empty | ||
|
|
||
| import pandas as pd | ||
| import numpy as np | ||
| import ros_numpy as ros_np | ||
| import threading | ||
| from copy import deepcopy | ||
|
|
||
| class ObostacleWorld: | ||
| def __init__(self): | ||
| self.sdf_version = rospy.get_param('~gazebo/sdf_version', 1.4) | ||
|
|
||
| self.m = rospy.get_param('~gazebo/obs/mass', 50.0) | ||
| self.h = rospy.get_param('~gazebo/obs/height', 2.0) | ||
|
|
||
| self.quadrotor_pos = None | ||
| self.quadrotor_rot = None | ||
| self.quadrotor_r = rospy.get_param('~quadrotor/radius', 0.3) | ||
| self.odom_sub = rospy.Subscriber('uav/cog/odom', Odometry, self.odomCb) | ||
| self.collision_pub = rospy.Publisher('collision_flag', Empty, queue_size = 1) | ||
|
|
||
| self.lock = threading.Lock() | ||
|
|
||
| rate = rospy.get_param('~rate', 40.0) # Hz | ||
| rospy.Timer(rospy.Duration(1.0/rate), self.mainProcess) | ||
|
|
||
| # debug | ||
| # r = 0.5 | ||
| # p = [0, 0, 0] # x, y, z | ||
| # name = 'obj1' | ||
| # self.spwan_obstacle(name, self.m, p, r, self.h) | ||
| # return | ||
|
|
||
| file = rospy.get_param('~obstacle_world_file') | ||
| if file is None: | ||
| rospy.logerr("no valid obsracle world file") | ||
| return | ||
|
|
||
| df = pd.read_csv(file, header=None) | ||
| self.obs = dict() | ||
| for i in range(len(df)): | ||
| name = 'obj' + str(i+1) | ||
| self.obs[name] = {'p': np.array(df.loc[i, 1:3].tolist()), 'r': df.at[0,8]} | ||
| self.obs[name]['p'][2] = self.h / 2 | ||
| self.spwanObstacle(name, self.m, self.obs[name]['p'], self.obs[name]['r'], self.h) | ||
|
|
||
| def odomCb(self, msg): | ||
| self.lock.acquire() | ||
| self.quadrotor_pos = ros_np.numpify(msg.pose.pose.position) | ||
| self.quadrotor_rot = ros_np.numpify(msg.pose.pose.orientation) | ||
| self.lock.release() | ||
|
|
||
| def mainProcess(self, event): | ||
| if self.quadrotor_pos is None: | ||
| return | ||
|
|
||
| self.lock.acquire() | ||
| quad_pos = deepcopy(self.quadrotor_pos) | ||
| quad_rot = deepcopy(self.quadrotor_rot) | ||
| self.lock.release() | ||
|
|
||
| # check the collision | ||
| for n, conf in self.obs.items(): | ||
| obs_pos = conf['p'] | ||
| obs_r = conf['r'] | ||
| cog_dist = np.linalg.norm(obs_pos[:2] - quad_pos[:2]) | ||
| min_dist = cog_dist - obs_r - self.quadrotor_r | ||
| rospy.logdebug("{}, pos: {}, quad pos: {}, cog dist: {}, min_dist: {}".format(n, obs_pos, quad_pos, cog_dist, min_dist)) | ||
| if min_dist < 0: | ||
| rospy.logwarn("{}, collision!! quadrotor: {}, obstacle: {} ".format(n, quad_pos, obs_pos)) | ||
| self.collision_pub.publish(Empty()) | ||
|
|
||
| # publish the obstacle position using LaserScan | ||
|
|
||
| def spwanObstacle(self, name, m, p, r, h): | ||
|
|
||
| rospy.wait_for_service('/gazebo/delete_model') | ||
| try: | ||
| delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) | ||
| resp = delete_model(name) | ||
| except rospy.ServiceException as e: | ||
| print("Service call failed: %s" % e) | ||
|
|
||
| model_xml = "<sdf version='{}'>".format(self.sdf_version) + \ | ||
| "<model name='{}'>".format(name) + \ | ||
| "<allow_auto_disable>true</allow_auto_disable>" + \ | ||
| "<pose> 0 0 0 0 0 0 </pose>" + \ | ||
| "<link name='link'>" + \ | ||
| "<velocity_decay>" + \ | ||
| "<linear>0.01</linear>" + \ | ||
| "<angular>0.01</angular>" + \ | ||
| "</velocity_decay>" + \ | ||
| "<inertial><mass> {} </mass>".format(m) + \ | ||
| "<inertia>" + \ | ||
| "<ixx> {} </ixx>".format(r**2/4.0 + h**2/12.0 * m) +\ | ||
| "<iyy> {} </iyy>".format(r**2/4.0 + h**2/12.0 * m) + \ | ||
| "<izz> {} </izz>".format(r**2/2.0 * m) + \ | ||
| "<ixy> 0.0 </ixy>" + \ | ||
| "<ixz> 0.0 </ixz>" + \ | ||
| "<iyz> 0.0 </iyz>" + \ | ||
| "</inertia>" + \ | ||
| "</inertial>" + \ | ||
| "<collision name='collision'>" + \ | ||
| "<geometry>" + \ | ||
| "<cylinder>" + \ | ||
| "<radius> {} </radius>".format(r) + \ | ||
| "<length> {} </length>".format(h) + \ | ||
| "</cylinder>" + \ | ||
| "</geometry>" + \ | ||
| "</collision>" + \ | ||
| "<visual name='visual'>" + \ | ||
| "<geometry>" + \ | ||
| "<cylinder>" + \ | ||
| "<radius> {} </radius>".format(r) + \ | ||
| "<length> {} </length>".format(h) + \ | ||
| "</cylinder>" + \ | ||
| "</geometry>" + \ | ||
| "<material>" + \ | ||
| "<ambient> 0.4 0.2 0.0 1 </ambient>" + \ | ||
| "<diffuse> 0.4 0.2 0.0 1 </diffuse>" + \ | ||
| "<specular>0.1 0.1 0.1 1</specular>" + \ | ||
| "<emissive>0 0 0 0</emissive>" + \ | ||
| "</material>" + \ | ||
| "</visual>" + \ | ||
| "</link>" + \ | ||
| "</model>" + \ | ||
| "</sdf>" | ||
|
|
||
| ret = gazebo_interface.spawn_sdf_model_client(name, model_xml, "obstacle", Pose(Point(p[0], p[1], p[2]), Quaternion(0,0,0,1)), '', '/gazebo') | ||
|
|
||
| return ret | ||
|
|
||
|
|
||
| if __name__=="__main__": | ||
| rospy.init_node("obsracle_world") | ||
|
|
||
| world = ObostacleWorld() | ||
| rospy.spin() |
Oops, something went wrong.