diff --git a/aerial_robot_simulation/launch/simulation.launch b/aerial_robot_simulation/launch/simulation.launch
index 3d575f606..4df4b6d3f 100644
--- a/aerial_robot_simulation/launch/simulation.launch
+++ b/aerial_robot_simulation/launch/simulation.launch
@@ -28,6 +28,4 @@
-
diff --git a/robots/agile_multirotor/config/obstacle_world/world1.csv b/robots/agile_multirotor/config/obstacle_world/world1.csv
new file mode 100644
index 000000000..94c606914
--- /dev/null
+++ b/robots/agile_multirotor/config/obstacle_world/world1.csv
@@ -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
diff --git a/robots/agile_multirotor/config/rviz_config b/robots/agile_multirotor/config/rviz_config
index 3ae22a2ab..909d8bb30 100644
--- a/robots/agile_multirotor/config/rviz_config
+++ b/robots/agile_multirotor/config/rviz_config
@@ -8,8 +8,9 @@ Panels:
- /Status1
- /TF1
- /RobotModel1
+ - /DepthCloud1/Auto Size1
Splitter Ratio: 0.5
- Tree Height: 1251
+ Tree Height: 1808
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -28,7 +29,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: ""
+ SyncSource: DepthCloud
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -65,6 +66,10 @@ Visualization Manager:
Value: true
multirotor/main_body:
Value: true
+ multirotor/rgbd_camera_frame:
+ Value: true
+ multirotor/rgbd_camera_optical_frame:
+ Value: true
multirotor/root:
Value: true
multirotor/thrust1:
@@ -90,6 +95,9 @@ Visualization Manager:
multirotor/main_body:
multirotor/fc:
{}
+ multirotor/rgbd_camera_frame:
+ multirotor/rgbd_camera_optical_frame:
+ {}
multirotor/thrust1:
{}
multirotor/thrust2:
@@ -119,6 +127,14 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
+ rgbd_camera_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ rgbd_camera_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
root:
Alpha: 1
Show Axes: false
@@ -149,6 +165,42 @@ Visualization Manager:
Update Interval: 0
Value: true
Visual Enabled: true
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: ""
+ Color Transformer: RGB8
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /multirotor/rgbd/depth/depth_registered
+ Depth Map Transport Hint: raw
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: DepthCloud
+ Occlusion Compensation:
+ Occlusion Time-Out: 30
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -177,7 +229,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 5.862760066986084
+ Distance: 14.51597785949707
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -200,10 +252,10 @@ Visualization Manager:
Window Geometry:
Displays:
collapsed: false
- Height: 1548
+ Height: 2105
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000016a0000056efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000056e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000056efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000056e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000026e0000056e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a0000079bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000079b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000079bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000079b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000ebd0000003efc0100000002fb0000000800540069006d0065010000000000000ebd000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000c380000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -212,6 +264,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 1267
- X: 957
- Y: 0
+ Width: 3773
+ X: 67
+ Y: 27
diff --git a/robots/agile_multirotor/launch/bringup.launch b/robots/agile_multirotor/launch/bringup.launch
index 69ee75b56..ca98fe617 100644
--- a/robots/agile_multirotor/launch/bringup.launch
+++ b/robots/agile_multirotor/launch/bringup.launch
@@ -6,7 +6,7 @@
-
+
@@ -82,7 +82,6 @@
-
@@ -92,4 +91,10 @@
######## work around to evoke robot model update #######
+ ####### collision world ############
+
+
+
+
+
diff --git a/robots/agile_multirotor/scripts/obstacle_world.py b/robots/agile_multirotor/scripts/obstacle_world.py
new file mode 100755
index 000000000..7dcdbc472
--- /dev/null
+++ b/robots/agile_multirotor/scripts/obstacle_world.py
@@ -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 = "".format(self.sdf_version) + \
+ "".format(name) + \
+ "true" + \
+ " 0 0 0 0 0 0 " + \
+ "" + \
+ "" + \
+ "0.01" + \
+ "0.01" + \
+ "" + \
+ " {} ".format(m) + \
+ "" + \
+ " {} ".format(r**2/4.0 + h**2/12.0 * m) +\
+ " {} ".format(r**2/4.0 + h**2/12.0 * m) + \
+ " {} ".format(r**2/2.0 * m) + \
+ " 0.0 " + \
+ " 0.0 " + \
+ " 0.0 " + \
+ "" + \
+ "" + \
+ "" + \
+ "" + \
+ "" + \
+ " {} ".format(r) + \
+ " {} ".format(h) + \
+ "" + \
+ "" + \
+ "" + \
+ "" + \
+ "" + \
+ "" + \
+ " {} ".format(r) + \
+ " {} ".format(h) + \
+ "" + \
+ "" + \
+ "" + \
+ " 0.4 0.2 0.0 1 " + \
+ " 0.4 0.2 0.0 1 " + \
+ "0.1 0.1 0.1 1" + \
+ "0 0 0 0" + \
+ "" + \
+ "" + \
+ "" + \
+ "" + \
+ ""
+
+ 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()
diff --git a/robots/agile_multirotor/urdf/quadrotor.gazebo.xacro b/robots/agile_multirotor/urdf/quadrotor.gazebo.xacro
index 1e9197fac..8f3a2f01a 100644
--- a/robots/agile_multirotor/urdf/quadrotor.gazebo.xacro
+++ b/robots/agile_multirotor/urdf/quadrotor.gazebo.xacro
@@ -2,7 +2,7 @@
-
+
@@ -33,4 +33,78 @@
Gazebo/Black
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 20.0
+
+ 1.57
+
+ 1280
+ 720
+ RGB
+
+
+ 0.02
+ 300
+
+
+ gaussian
+ 0.0
+ 0.007
+
+
+
+ $(arg robot_name)
+ 0.06
+ true
+ 0.0
+ rgbd
+ rgb/image_rect_color
+ rgb/camera_info
+ depth/depth_registered
+ depth/camera_info
+ point_cloud/cloud_registered
+ rgbd_camera_optical_frame
+ 0.05
+ 10.0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+ 0
+
+
+
+