diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0ebecc1fafa85d078e25218365f89651d0cccb3a..629383225db6b8ff3c70bb40ae789f1c5b548783 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -78,6 +78,7 @@ add_action_files(
short_grippers.action
long_grippers.action
coverage.action
+ gotoposition_sensor.action
)
## Generate added messages and services with any dependencies listed here
diff --git a/src/launch/autonomous_offboard_descend_test.launch b/src/launch/autonomous_offboard_descend_test.launch
old mode 100644
new mode 100755
index bba731b6d9bcd73c656b043c21bdbafcc36bb723..490c4f739e7ca50f0cb510fd2c291bfb8edd3645
--- a/src/launch/autonomous_offboard_descend_test.launch
+++ b/src/launch/autonomous_offboard_descend_test.launch
@@ -2,7 +2,6 @@
<node name="position_control" pkg="autonomous_offboard" type="position_control.py" output="screen" />
<node name="action_controller_descend" pkg="autonomous_offboard" type="action_controller_descend.py" output="screen" />
<node name="goto_position_server" pkg="autonomous_offboard" type="goto_position_server.py" output="screen" />
- <node name="tensorflow_detection" pkg="autonomous_offboard" type="tensorflow_hw_mvBlueFox_detection.py" output="screen" />
+ <node name="goto_position_vel_server" pkg="autonomous_offboard" type="goto_position_vel_server.py" output="screen" />
<node name="descend_on_object_server" pkg="autonomous_offboard" type="descend_on_object_server.py" output="screen" />
- <node name="obstacle_detection_control" pkg="autonomous_offobard" type="obstacle_detection_control.py" output="screen" />
</launch>
diff --git a/src/launch/autonomous_offboard_pid.launch b/src/launch/autonomous_offboard_pid.launch
index b6a471b0a96f2060916455f423ce9251802d9493..fff488eb3b433244eb0e0e866456f4f99f1bb803 100644
--- a/src/launch/autonomous_offboard_pid.launch
+++ b/src/launch/autonomous_offboard_pid.launch
@@ -3,5 +3,4 @@
<node name="action_controller_pid" pkg="autonomous_offboard" type="action_controller_pid.py" output="screen" />
<node name="goto_position_server" pkg="autonomous_offboard" type="goto_position_server.py" output="screen" />
<node name="goto_position_vel_server" pkg="autonomous_offboard" type="goto_position_vel_server.py" output="screen" />
- <node name="obstacle_detection_control" pkg="simulation_control" type="obstacle_detection_control.py" output="screen" />
</launch>
diff --git a/src/launch/autonomous_offboard_takeoff_test.launch b/src/launch/autonomous_offboard_takeoff_test.launch
index 5b5f06ce3abffc87de8347cd701c0cad7e4689c2..5fd0e4b5a6ff777268c5a4355e5435313d84032d 100644
--- a/src/launch/autonomous_offboard_takeoff_test.launch
+++ b/src/launch/autonomous_offboard_takeoff_test.launch
@@ -2,5 +2,5 @@
<node name="position_control" pkg="autonomous_offboard" type="position_control.py" output="screen" />
<node name="action_controller_takeoff_test" pkg="autonomous_offboard" type="action_controller_takeoff_test.py" output="screen" />
<node name="goto_position_server" pkg="autonomous_offboard" type="goto_position_server.py" output="screen" />
- <node name="takeoff_curr_server" pkg="autonomous_offboard" type="takeoff_curr_server.py" output="screen" />
+ <node name="goto_position_vel_server" pkg="autonomous_offboard" type="goto_position_vel_server.py" output="screen" />
</launch>
diff --git a/src/scripts/VelocityController.py b/src/scripts/VelocityController.py
index e77d9d72af389be3d856218b57015cd4f25d74d0..b0608cfcc8983266e3c2551641fc424767815f75 100755
--- a/src/scripts/VelocityController.py
+++ b/src/scripts/VelocityController.py
@@ -88,6 +88,11 @@ class VelocityController:
self.yaw.setKi(ki)
self.yaw.setKd(kd)
self.yaw.setMaxO(output)
+ def set_xy_vel(self, output):
+ self.X.maxOut(output)
+ self.Y.maxOut(output)
+ def set_z_vel(self,output):
+ self.Z.maxOut(output)
diff --git a/src/scripts/VelocityController.pyc b/src/scripts/VelocityController.pyc
index cd0db68d61e1bdd6b53ff4bbd5458813bb88786b..50bdbfac4af2abef38257b04a520cd87aabf4020 100644
Binary files a/src/scripts/VelocityController.pyc and b/src/scripts/VelocityController.pyc differ
diff --git a/src/scripts/action_controller_descend.py b/src/scripts/action_controller_descend.py
index 3548b253df4ab44365e6ee7adeb9d75c7c072aa0..64d53dadc394f7190e0394c5bc9673e6c72f8d45 100755
--- a/src/scripts/action_controller_descend.py
+++ b/src/scripts/action_controller_descend.py
@@ -1,41 +1,37 @@
#!/usr/bin/env python
import roslib
-roslib.load_manifest('simulation_control')
+#roslib.load_manifest('autonomous_offboard')
import rospy
import actionlib
import mavros_state
import time
-from autonomous_offboard.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal, long_grippersAction, long_grippersGoal, coverageAction, coverageGoal
+from autonomous_offboard.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal,goto_position_velAction, goto_position_velGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal, long_grippersAction, long_grippersGoal, coverageAction, coverageGoal
from std_msgs.msg import Float32
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
rospy.init_node('action_controller_descend')
+ goto_position_vel_client = actionlib.SimpleActionClient('goto_position_vel', goto_position_velAction)
+ goto_position_vel_client.wait_for_server()
+ goto_position_vel_goal = goto_position_velGoal()
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
+ print 'Takeoff'
mv_state.arm(True)
- rospy.loginfo("Taking off")
- goto_position_client = actionlib.SimpleActionClient('goto_position', goto_positionAction)
- goto_position_client.wait_for_server()
- goto_position_goal = goto_positionGoal()
- goto_position_goal.destination.pose.position.z = 2
- goto_position_client.send_goal(goto_position_goal)
- goto_position_client.wait_for_result()
- rospy.loginfo("Takeoff succeded")
+ print 'Sleeping 20 sec'
+ time.sleep(20)
- time.sleep(5)
-
- rospy.loginfo("First position")
- goto_position_goal.destination.pose.position.x = -2
- goto_position_goal.destination.pose.position.y = 0
- goto_position_goal.destination.pose.position.z = 2
- goto_position_client.send_goal(goto_position_goal)
- goto_position_client.wait_for_result()
+ rospy.loginfo("Detect position")
+ goto_position_vel_goal.destination.pose.position.x = 4
+ goto_position_vel_goal.destination.pose.position.y = 0
+ goto_position_vel_goal.destination.pose.position.z = 3
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
+ goto_position_vel_client.wait_for_result()
time.sleep(2)
@@ -53,19 +49,40 @@ if __name__ == '__main__':
else:
rospy.loginfo("Couldnt land exiting")
- time.sleep(3)
-
- print 'Setting offboard'
- mv_state.set_mode('OFFBOARD')
- print 'Arming vehicle'
- mv_state.arm(True)
-
- time.sleep(1)
-
- rospy.loginfo("Going Home")
- goto_position_goal.destination.pose.position.x = 0
- goto_position_goal.destination.pose.position.y = 0
- goto_position_goal.destination.pose.position.z = 5
- goto_position_client.send_goal(goto_position_goal)
- goto_position_client.wait_for_result()
- mv_state.land(0.0)
+ # time.sleep(3)
+ #
+ # print 'Setting offboard'
+ # mv_state.set_mode('OFFBOARD')
+ # print 'Arming vehicle'
+ # mv_state.arm(True)
+ #
+ # time.sleep(5)
+ #
+ # rospy.loginfo("Takeoff")
+ # goto_position_goal.destination.pose.position.x = 0
+ # goto_position_goal.destination.pose.position.y = 0
+ # goto_position_goal.destination.pose.position.z = 3
+ # goto_position_client.send_goal(goto_position_goal)
+ # goto_position_client.wait_for_result()
+ #
+ # time.sleep(5)
+ #
+ # rospy.loginfo("Going Home")
+ # goto_position_goal.destination.pose.position.x = -5
+ # goto_position_goal.destination.pose.position.y = 0
+ # goto_position_goal.destination.pose.position.z = 3
+ # goto_position_client.send_goal(goto_position_goal)
+ # goto_position_client.wait_for_result()
+ #
+ # time.sleep(5)
+ #
+ # rospy.loginfo("Landing")
+ # goto_position_vel_goal.destination.pose.position.x = 0
+ # goto_position_vel_goal.destination.pose.position.y = 0
+ # goto_position_vel_goal.destination.pose.position.z = -0.1
+ # goto_position_vel_client.send_goal(goto_position_vel_goal)
+ # goto_position_vel_client.wait_for_result()
+ # rospy.loginfo("Trying to land, 10 second sleep")
+ # time.sleep(10)
+ #
+ # mv_state.arm(False)
diff --git a/src/scripts/action_controller_pid.py b/src/scripts/action_controller_pid.py
index 11607d0fa3f6509bb414e0b307f704ced65362a5..fadead0d0048476ed41be0e50c3b4371612e9cbb 100755
--- a/src/scripts/action_controller_pid.py
+++ b/src/scripts/action_controller_pid.py
@@ -19,8 +19,9 @@ if __name__ == '__main__':
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
mv_state.arm(True)
- time.sleep(5)
rospy.loginfo("Taking off")
+ time.sleep(10)
+
goto_position_client = actionlib.SimpleActionClient('goto_position', goto_positionAction)
goto_position_client.wait_for_server()
goto_position_goal = goto_positionGoal()
@@ -29,30 +30,36 @@ if __name__ == '__main__':
goto_position_client.wait_for_result()
rospy.loginfo("Takeoff succeded")
- time.sleep(5)
- rospy.loginfo("Z PID")
- ######Z PID#########
+ # time.sleep(5)
+ # rospy.loginfo("Z PID")
+ # print("sleeping 5 sec start Rosbagging. ZPID")
+ # time.sleep(5)
+ # ######Z PID#########
goto_position_vel_client = actionlib.SimpleActionClient('goto_position_vel', goto_position_velAction)
goto_position_vel_client.wait_for_server()
goto_position_vel_goal = goto_position_velGoal()
- goto_position_vel_goal.destination.pose.position.x = 0
- goto_position_vel_goal.destination.pose.position.y = 0
- goto_position_vel_goal.destination.pose.position.z = 3
- goto_position_vel_client.send_goal(goto_position_vel_goal)
- goto_position_vel_client.wait_for_result()
-
- time.sleep(10)
-
- ###STABALIZE
- rospy.loginfo("Stabalize")
- goto_position_goal.destination.pose.position.x = 0
- goto_position_goal.destination.pose.position.y = 0
- goto_position_goal.destination.pose.position.z = 2
- goto_position_client.send_goal(goto_position_goal)
- goto_position_client.wait_for_result()
+ # goto_position_vel_goal.destination.pose.position.x = 0
+ # goto_position_vel_goal.destination.pose.position.y = 0
+ # goto_position_vel_goal.destination.pose.position.z = 3
+ # goto_position_vel_client.send_goal(goto_position_vel_goal)
+ # goto_position_vel_client.wait_for_result()
+ #
+ # time.sleep(10)
+ # print("z pid done, stop rosbag")
+ # time.sleep(5)
+ # ###STABALIZE
+ # rospy.loginfo("Stabalize")
+ # goto_position_goal.destination.pose.position.x = 0
+ # goto_position_goal.destination.pose.position.y = 0
+ # goto_position_goal.destination.pose.position.z = 2
+ # goto_position_client.send_goal(goto_position_goal)
+ # goto_position_client.wait_for_result()
time.sleep(5)
+
rospy.loginfo("X PID")
+ print("sleeping 5 sec start Rosbagging. XPID")
+ time.sleep(5)
#######X PID##############
goto_position_vel_goal.destination.pose.position.x = 1
goto_position_vel_goal.destination.pose.position.y = 0
@@ -61,19 +68,22 @@ if __name__ == '__main__':
goto_position_vel_client.wait_for_result()
time.sleep(10)
-
+ print("x pid done, stop rosbag")
+ time.sleep(5)
###STABALIZE
rospy.loginfo("Stabalize")
- goto_position_goal.destination.pose.position.x = 0
+ goto_position_goal.destination.pose.position.x = -1
goto_position_goal.destination.pose.position.y = 0
goto_position_goal.destination.pose.position.z = 2
goto_position_client.send_goal(goto_position_goal)
goto_position_client.wait_for_result()
time.sleep(5)
-
+ #
#######Y PID##############
rospy.loginfo("Y PID")
+ print("sleeping 5 sec start Rosbagging. YPID")
+ time.sleep(5)
goto_position_vel_goal.destination.pose.position.x = 0
goto_position_vel_goal.destination.pose.position.y = 1
goto_position_vel_goal.destination.pose.position.z = 2
@@ -81,11 +91,12 @@ if __name__ == '__main__':
goto_position_vel_client.wait_for_result()
time.sleep(10)
-
+ print("y pid done, stop rosbag")
+ time.sleep(5)
###STABALIZE
rospy.loginfo("Stabalize")
goto_position_goal.destination.pose.position.x = 0
- goto_position_goal.destination.pose.position.y = 0
+ goto_position_goal.destination.pose.position.y = -1
goto_position_goal.destination.pose.position.z = 2
goto_position_client.send_goal(goto_position_goal)
goto_position_client.wait_for_result()
diff --git a/src/scripts/action_controller_takeoff_test.py b/src/scripts/action_controller_takeoff_test.py
index fd8d3a7e34cbb9cd67bb2a89de8beae6a1e834bd..69c8c0aaabba1dfb5656324bc1f251eb4140d1ef 100755
--- a/src/scripts/action_controller_takeoff_test.py
+++ b/src/scripts/action_controller_takeoff_test.py
@@ -7,13 +7,16 @@ import actionlib
import mavros_state
import time
-from autonomous_offboard.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal,takeoff_currAction, takeoff_currGoal, long_grippersAction, long_grippersGoal, coverageAction, coverageGoal
+from autonomous_offboard.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal,goto_position_velAction, goto_position_velGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal, long_grippersAction, long_grippersGoal, coverageAction, coverageGoal
from std_msgs.msg import Float32
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
rospy.init_node('action_controller_takeoff_test')
+ goto_position_vel_client = actionlib.SimpleActionClient('goto_position_vel', goto_position_velAction)
+ goto_position_vel_client.wait_for_server()
+ goto_position_vel_goal = goto_position_velGoal()
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
@@ -22,5 +25,23 @@ if __name__ == '__main__':
mv_state.arm(True)
print 'Sleeping 20 sec'
time.sleep(20)
- print 'Landing'
- mv_state.land(0.0)
+
+ rospy.loginfo("change true height position")
+ goto_position_vel_goal.destination.pose.position.x = 0
+ goto_position_vel_goal.destination.pose.position.y = 0
+ goto_position_vel_goal.destination.pose.position.z = 7
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
+ goto_position_vel_client.wait_for_result()
+ rospy.loginfo("10 second sleep before landing")
+ time.sleep(10)
+ rospy.loginfo("Landing")
+ goto_position_vel_goal.destination.pose.position.x = 0
+ goto_position_vel_goal.destination.pose.position.y = 0
+ goto_position_vel_goal.destination.pose.position.z = -0.1
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
+ goto_position_vel_client.wait_for_result()
+ rospy.loginfo("Trying to land, 5 second sleep")
+ time.sleep(2)
+
+ mv_state.arm(False)
+
diff --git a/src/scripts/config.yml b/src/scripts/config.yml
index e405a562f02b3b93c4f63fa1621b5436ca285e06..0662d22a20f2763b5e700ae7b11e7d6e28efb317 100644
--- a/src/scripts/config.yml
+++ b/src/scripts/config.yml
@@ -4,8 +4,8 @@ video_input: 0 # Input Must be OpenCV readable #0 for webcam
visualize: True # Disable for performance increase
vis_text: True # Display fps on visualization stream
max_frames: 5000 # only used if visualize==False
-width: 600 # OpenCV only supports 4:3 formats others will be converted
-height: 600 # 600x600 leads to 640x480
+width: 2000 # OpenCV only supports 4:3 formats others will be converted
+height: 2000 # 600x600 leads to 640x480
fps_interval: 5 # Interval [s] to print fps of the last interval in console
det_interval: 500 # intervall [frames] to print detections to console
det_th: 0.5 # detection threshold for det_intervall
@@ -24,6 +24,6 @@ num_trackers: 5 # Max number of objects to track
## Model
model_name: 'ssd_mobilenet_v11_coco'
-model_path: '/home/pateman/catkin_ws/src/simulation_control/src/scripts/models/ssd_mobilenet_v11_coco/frozen_inference_graph.pb'
-label_path: '/home/pateman/TensorflowProjects/Test5UavFrame/data/object-detection.pbtxt'
-num_classes: 1
+model_path: '/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/models/ssd_mobilenet_v11_coco/frozen_inference_graph.pb'
+label_path: '/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/object-detection.pbtxt'
+num_classes: 2
diff --git a/src/scripts/coverage_server.py b/src/scripts/coverage_server.py
index 68b935ed45a00fd02d2c51f9e289cc9cdc3fa121..642bd15903077120938af697a1557cc6bd29a2b1 100755
--- a/src/scripts/coverage_server.py
+++ b/src/scripts/coverage_server.py
@@ -5,27 +5,33 @@ import autonomous_offboard.msg
import numpy as np
import time
import math
-from geometry_msgs.msg import PoseStamped
-from simulation_control.msg import goto_positionAction, goto_positionGoal, detect_objectAction, detect_objectGoal
+from geometry_msgs.msg import PoseStamped, Point
+from std_msgs.msg import Float32
+from autonomous_offboard.msg import detect_objectAction, detect_objectGoal, goto_position_velAction, goto_position_velGoal
class coverage_server():
def __init__(self):
# variables
+ self.cur_pose = PoseStamped()
self.waypoints = np.empty((0,3))
self.destination = PoseStamped()
self.angleOfViewWidth = 37
self.angleOfViewHeight = 47
- self.areaWidth = 50
- self.areaHeight = 50
+ self.areaWidth = 20
+ self.areaHeight = 20
self.searchHeight = 10
self.detectedPos = PoseStamped()
self.detectedPos.pose.position.x = float("inf")
- self.result = simulation_control.msg.coverageResult()
+ self.result = autonomous_offboard.msg.coverageResult()
+ rospy.Subscriber('/tensorflow_detection/cam_point', Point, self.detection_callback)
+ rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.cur_pose_cb)
- self.goto_position_client = actionlib.SimpleActionClient('goto_position', goto_positionAction)
- self.detect_object_client = actionlib.SimpleActionClient('detect_object', detect_objectAction)
+ self.set_xy_vel = rospy.Publisher('/position_control/set_xy_vel', Float32, queue_size=10)
+
+ self.goto_position_client = actionlib.SimpleActionClient('goto_position', goto_position_velAction)
+ #self.detect_object_client = actionlib.SimpleActionClient('detect_object', detect_objectAction)
self.rate = rospy.Rate(20)
self.action_server = actionlib.SimpleActionServer('coverage',
autonomous_offboard.msg.coverageAction,
@@ -33,12 +39,14 @@ class coverage_server():
self.action_server.start()
def execute_cb(self, goal):
- goto_position_goal = goto_positionGoal()
+ xyVel = Float32()
+ xyVel.data = 1
+ self.set_xy_vel.publish(xyVel)
+ goto_position_goal = goto_position_velGoal()
self.goto_position_client.wait_for_server()
- self.detect_object_client.wait_for_server()
- goto_position_goal = goto_positionGoal()
+ #self.detect_object_client.wait_for_server()
self.destination = goal.destination
- self.calculate_waypoints()
+ self.calculate_waypoints2()
idx= 0
while(self.detectedPos.pose.position.x == float("inf")):
goto_position_goal.destination.pose.position.x = self.waypoints.item((idx, 0))
@@ -47,20 +55,20 @@ class coverage_server():
self.goto_position_client.send_goal(goto_position_goal)
self.goto_position_client.wait_for_result()
idx = idx + 1
- if idx == self.waypoints.shape[0]:
+ if idx == self.waypoints.shape[0] or self.detectedPos.pose.position.x != float("inf"):
idx = 0
-
- detect_object_goal = detect_objectGoal()
- self.detect_object_client.send_goal(detect_object_goal)
- self.detect_object_client.wait_for_result()
- self.detectedPos = self.detect_object_client.get_result().detected_position
- time.sleep(0.1)
- if self.detectedPos.pose.position.x != float("inf"):
-
self.result.detected_object_pos = self.detectedPos
self.result.detected_object_pos.pose.position.z = self.searchHeight
self.action_server.set_succeeded(self.result)
+ #detect_object_goal = detect_objectGoal()
+ #self.detect_object_client.send_goal(detect_object_goal)
+ #self.detect_object_client.wait_for_result()
+ #self.detectedPos = self.detect_object_client.get_result().detected_position
+ time.sleep(0.1)
+
+
+
#calculate waypoints
@@ -91,10 +99,62 @@ class coverage_server():
self.waypoints = np.asarray(tempArr)
rospy.loginfo(self.waypoints)
+ def calculate_waypoints2(self):
+ tempArr = []
+ self.cameraWidth = 2 * self.searchHeight * math.tan(math.radians(self.angleOfViewWidth / 2))
+ self.cameraHeight = 2 * self.searchHeight * math.tan(math.radians(self.angleOfViewHeight / 2))
+ startPointX = self.destination.pose.position.x - self.areaWidth / 2 + self.cameraWidth / 2
+ startPointY = self.destination.pose.position.y - self.areaHeight / 2 + self.cameraHeight / 2
+ self.nrOfStepsHeight = int(math.floor(self.areaHeight / self.cameraHeight))
+ self.nrOfStepsWidth = int(math.floor(self.areaWidth / self.cameraWidth))
+
+ tempArr.append([startPointX, startPointY, self.searchHeight])
+
+ for i in range(0, self.nrOfStepsWidth):
+ for j in range(0, self.nrOfStepsHeight):
+
+ if j == self.nrOfStepsHeight - 1:
+ _last_steplength = self.areaHeight - abs(self.nrOfStepsHeight * self.cameraHeight)
+ if self.cameraHeight > 0:
+ tempArr.append([0, _last_steplength, self.searchHeight])
+ else:
+ tempArr.append([0, -_last_steplength, self.searchHeight])
+ else:
+ tempArr.append([0, self.cameraHeight, self.searchHeight])
+ if i == self.nrOfStepsWidth-1:
+ self.cameraHeight = self.cameraHeight * (-1) # make it change direction for zig-zag pattern
+ _last_steplength = self.areaHeight - abs(self.nrOfStepsWidth * self.cameraWidth)
+ if self.cameraWidth > 0:
+ tempArr.append([_last_steplength, 0, self.searchHeight])
+ else:
+ tempArr.append([-_last_steplength, 0, self.searchHeight])
+ for j in range(0, self.nrOfStepsHeight):
+
+ if j == self.nrOfStepsHeight - 1:
+ _last_steplength = self.areaHeight - abs(self.nrOfStepsHeight * self.cameraHeight)
+ if self.cameraHeight > 0:
+ tempArr.append([0, _last_steplength, self.searchHeight])
+ else:
+ tempArr.append([0, -_last_steplength, self.searchHeight])
+ else:
+ tempArr.append([0, self.cameraHeight, self.searchHeight])
+
+
+ elif i != self.nrOfStepsWidth:
+ self.cameraHeight = self.cameraHeight * (-1) # make it change direction for zig-zag pattern
+ tempArr.append([self.cameraWidth, 0, self.searchHeight])
+ self.waypoints = np.asarray(tempArr)
+ rospy.loginfo(self.waypoints)
+ def detection_callback(self, data):
+ if (data.x != float("inf")):
+ self.detectedPos = self.cur_pose
+ self.detectedPos.pose.position.x = self.detectedPos.pose.position.x + data.x
+ self.detectedPos.pose.position.y = self.detectedPos.pose.position.y + data.y
-
+ def cur_pose_cb(self, data):
+ self.cur_pose = data
if __name__ == '__main__':
try:
diff --git a/src/scripts/descend_on_object_server.py b/src/scripts/descend_on_object_server.py
index 8b92f4cf6c6ef2503c74bd8bb409a253ba3321d7..dfdff49034672000f6b3ff0a4e83727ade5fca79 100755
--- a/src/scripts/descend_on_object_server.py
+++ b/src/scripts/descend_on_object_server.py
@@ -3,7 +3,7 @@ import rospy
import actionlib
import autonomous_offboard.msg
from geometry_msgs.msg import PoseStamped, Point
-from std_msgs.msg import Bool, String
+from std_msgs.msg import Bool, String, Float32
import time
@@ -19,15 +19,19 @@ class descend_on_object_server():
# publishers
self.mode_control = rospy.Publisher('/position_control/set_mode', String, queue_size=10)
self.vel_control = rospy.Publisher('/position_control/set_velocity', PoseStamped, queue_size=10)
+ self.set_xy_vel = rospy.Publisher('/position_control/set_xy_vel', Float32, queue_size=10)
+ self.set_z_vel = rospy.Publisher('/position_control/set_z_vel', Float32, queue_size=10)
# subscribers
rospy.Subscriber('/tensorflow_detection/cam_point', Point, self.get_cam_pos_callback)
rospy.Subscriber('/tensorflow_detection/cam_point_center', Point, self.get_cam_pos_center_callback)
- rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
+ #rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
+ rospy.Subscriber('/position_control/Real_pose', PoseStamped, self._real_pose_callback)
+
self.rate = rospy.Rate(20)
- self.result = simulation_control.msg.descend_on_objectResult()
+ self.result = autonomous_offboard.msg.descend_on_objectResult()
self.action_server = actionlib.SimpleActionServer('descend_on_object',
autonomous_offboard.msg.descend_on_objectAction,
execute_cb=self.execute_cb,
@@ -37,6 +41,9 @@ class descend_on_object_server():
self.action_server.start()
def execute_cb(self, goal):
+ xyVel = Float32()
+ xyVel.data = 0.2
+ self.set_xy_vel.publish(xyVel)
rospy.loginfo("Starting to descend")
self.mode_control.publish('velctr')
rospy.sleep(0.1)
@@ -95,7 +102,7 @@ class descend_on_object_server():
print("Landing")
self.des_pose.pose.position.x = 0
self.des_pose.pose.position.y = 0
- self.des_pose.pose.position.z = self.local_pose.pose.position.z - 0.4
+ self.des_pose.pose.position.z = -0.1
self.vel_control.publish(self.des_pose)
time.sleep(0.1)
while not self.target_reached:
@@ -121,11 +128,13 @@ class descend_on_object_server():
else:
self.detected = False
- def _local_pose_callback(self, data):
- self.local_pose = data
+ # def _local_pose_callback(self, data):
+ # self.local_pose = data
def distance_reached_cb(self, data):
self.target_reached = data.data
+ def _real_pose_callback(self, data):
+ self.local_pose = data
if __name__ == '__main__':
diff --git a/src/scripts/models/ssd_mobilenet_v11_coco/frozen_inference_graph.pb b/src/scripts/models/ssd_mobilenet_v11_coco/frozen_inference_graph.pb
index 28bdefba3a6de6bddad3f011d2fbba800cdf7778..9077a968d1d9f2691d5b4023c29b100619ec08c8 100644
Binary files a/src/scripts/models/ssd_mobilenet_v11_coco/frozen_inference_graph.pb and b/src/scripts/models/ssd_mobilenet_v11_coco/frozen_inference_graph.pb differ
diff --git a/src/scripts/position_control.py b/src/scripts/position_control.py
index dea674428a64bbd34636a343fbce968ed7f4b36d..32eb4cced771161d71020187613a112f0c141aef 100755
--- a/src/scripts/position_control.py
+++ b/src/scripts/position_control.py
@@ -1,11 +1,13 @@
#!/usr/bin/env python
-from geometry_msgs.msg import PoseStamped, TwistStamped, Point
+from geometry_msgs.msg import PoseStamped, TwistStamped, Point, Quaternion
import rospy
import math
+import tf
from std_msgs.msg import Float32, Bool, String
from VelocityController import VelocityController
from sensor_msgs.msg import Imu
import numpy as np
+import copy
class position_control():
@@ -17,6 +19,7 @@ class position_control():
self.lidar_height = 0.0
self.actual_height = Float32
self.actual_height = 0.0
+ self.real_pose = PoseStamped()
# ----------Subscribers----------#
rospy.Subscriber('/position_control/set_mode', String, self.set_mode_callback)
@@ -28,6 +31,9 @@ class position_control():
rospy.Subscriber('/position_control/set_y_pid', Point, self.set_y_pid)
rospy.Subscriber('/position_control/set_z_pid', Point, self.set_z_pid)
rospy.Subscriber('/position_control/set_yaw_pid', Point, self.set_yaw_pid)
+ #Set max output velocity on PID in velocity control
+ rospy.Subscriber('/position_control/set_xy_vel', Float32, self.set_xy_vel)
+ rospy.Subscriber('/position_control/set_z_vel', Float32, self.set_z_vel)
self.local_pose = PoseStamped()
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
@@ -52,16 +58,16 @@ class position_control():
self._velpose_msg = PoseStamped()
self._velpose_state = "velposctr"
- # self._velpos_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
- # self._velpos_msg = TwistStamped()
- # self._velpos_state = "velposctr"
+
self.dist = rospy.Publisher('/position_control/distance', Bool, queue_size=10)
- self.real_local_pose = rospy.Publisher('/position_control/local_pose', Float32, queue_size=10)
+ self._real_pose = rospy.Publisher('/position_control/Real_pose', PoseStamped, queue_size=10)
self.yawangle = rospy.Publisher('/position_control/Yawangle', Float32, queue_size=10)
self.pitchangle = rospy.Publisher('/position_control/Pitchangle', Float32, queue_size=10)
self.rollangle = rospy.Publisher('/position_control/Rollangle', Float32, queue_size=10)
+ self.pid_out_pub = rospy.Publisher('/position_control/pid_out', TwistStamped, queue_size=10)
+
self.current_publisher = self._pose_pub
self.current_message = self._pose_msg
# self._pose_msg.pose.position.z = 3
@@ -75,11 +81,11 @@ class position_control():
self.current_mode.data = 'posctr'
self.vController = VelocityController()
- self.vController.set_x_pid(1.0, 0.0, 0.0, 5) # 0.15 #MARCUS: 2.8, 0.913921, 0.0, 1
- self.vController.set_y_pid(1.0, 0.0, 0.0, 5) # 2.1, 0.713921, 0.350178 #MARCUS: 2.8, 0.913921, 0.0, 1
- self.vController.set_z_pid(1.0, 0.0, 0.0, 5) # 0.15 #MARCUS: 1.3, 2.4893, 0.102084, 1
+ self.vController.set_x_pid(1.0, 0.0, 0.0, 1) # 0.15 #MARCUS: 2.8, 0.913921, 0.0, 1
+ self.vController.set_y_pid(1.0, 0.0, 0.0, 1) # 2.1, 0.713921, 0.350178 #MARCUS: 2.8, 0.913921, 0.0, 1
+ self.vController.set_z_pid(1.0, 0.0, 0.0, 0.3) # 0.15 #MARCUS: 1.3, 2.4893, 0.102084, 1
# self.vController.set_yaw_pid(3.6,1.33333,0.1875,1)
- self.vController.set_yaw_pid(1, 1.33333, 0.1875, 1)
+ self.vController.set_yaw_pid(1, 0, 0, 1)#1, 1.33333, 0.1875, 1
print 'Init done'
while not rospy.is_shutdown():
@@ -87,17 +93,19 @@ class position_control():
self._pose_pub.publish(self._pose_msg)
elif self.current_mode.data == 'velctr':
self.vController.setTarget(self._vel_pose_msg.pose)
- self.des_vel = self.vController.update(self.local_pose)
+ self.des_vel = self.vController.update(self.real_pose)
self._vel_pub.publish(self.des_vel)
+ self.pid_out_pub.publish(self.des_vel)
elif self.current_mode.data == 'velposctr':
self.vController.setTarget(self._velpose_msg.pose)
- self.des_velpos = self.vController.update(self.local_pose)
+ self.des_velpos = self.vController.update(self.real_pose)
self._velpose_pub.publish(self.des_velpos)
+ self.pid_out_pub.publish(self.des_velpos)
else:
print "No such position mode"
- self.real_local_pose.publish(self.actual_height)
+ self._real_pose.publish(self.real_pose)
self.check_distance()
self.get_angles()
rate.sleep()
@@ -124,7 +132,9 @@ class position_control():
# print(vel_pose.pose)
self._vel_pose_msg.pose.position.x = self.local_pose.pose.position.x + vel_pose.pose.position.x
self._vel_pose_msg.pose.position.y = self.local_pose.pose.position.y + vel_pose.pose.position.y
+
self._vel_pose_msg.pose.position.z = vel_pose.pose.position.z
+ print(self._vel_pose_msg)
# print(self._vel_pose_msg.pose)
self._vel_pose_msg.pose.orientation.x = vel_pose.pose.orientation.x
self._vel_pose_msg.pose.orientation.y = vel_pose.pose.orientation.y
@@ -143,6 +153,8 @@ class position_control():
def _local_pose_callback(self, data):
self.local_pose = data
+ self.real_pose = copy.deepcopy(self.local_pose)
+ self.real_pose.pose.position.z = self.actual_height
def _local_velocity_callback(self, data):
@@ -159,16 +171,12 @@ class position_control():
Z = self.local_imu.orientation.z
W = self.local_imu.orientation.w
-
-
- roll = math.atan2(2 * (Y * Z + W * X), W * W - X * X - Y * Y + Z * Z)
- pitch = math.asin(-2 * (X * Z - W * Y))
-
+ (roll, pitch, Yaw) = tf.transformations.euler_from_quaternion([X, Y, Z, W])
Comp = -math.sin(pitch) * 0.22 # 0.22 = 22cm from rotation centrum
- self.actual_height = (self.lidar_height * (math.cos(pitch) * math.cos(roll))) - Comp
+ self.actual_height = ((self.lidar_height * (math.cos(pitch) * math.cos(roll))) - Comp)-0.3
def set_pose_callback(self, data):
self.set_pose(data)
@@ -191,16 +199,20 @@ class position_control():
def set_yaw_pid(self, data):
self.vController.set_yaw_pid(data.x, data.y, data.z)
+ def set_xy_vel(self, data):
+ self.vController.set_xy_vel(data)
+ def set_z_vel(self, data):
+ self.vController.set_z_vel(data)
+
+
def get_angles(self):
X = self.local_imu.orientation.x
Y = self.local_imu.orientation.y
Z = self.local_imu.orientation.z
W = self.local_imu.orientation.w
+ (roll, pitch, yaw) = tf.transformations.euler_from_quaternion([X, Y, Z, W])
- yaw = math.asin(2 * X * Y + 2 * Z * W)
- roll = math.atan2(2 * (Y * Z + W * X), W * W - X * X - Y * Y + Z * Z)
- pitch = math.asin(-2 * (X * Z - W * Y))
self.yawangle.publish(math.degrees(yaw)) # Yaw in degrees
self.pitchangle.publish(math.degrees(pitch)) # Pitch in degrees
@@ -212,12 +224,8 @@ class position_control():
boolvel = self.hover_velocity()
self.dist.publish(booldist and boolvel)
elif self.current_mode.data == 'velctr' or self.current_mode.data == 'velposctr':
- vel_pose_tot = PoseStamped()
- vel_pose_tot.pose.position.x = self._vel_pose_msg.pose.position.x
- vel_pose_tot.pose.position.y = self._vel_pose_msg.pose.position.y
- vel_pose_tot.pose.position.z = self._vel_pose_msg.pose.position.z
# print("target vel_pos: {}".format(vel_pose_tot))
- booldist = self.is_at_position(self.local_pose.pose.position, vel_pose_tot.pose.position, 0.2)
+ booldist = self.is_at_position(self.real_pose.pose.position, self._vel_pose_msg.pose.position, 0.2)
boolvel = self.hover_velocity()
self.dist.publish(booldist and boolvel)
diff --git a/src/scripts/tensorflow_hw_mvBlueFox_detection.py b/src/scripts/tensorflow_hw_mvBlueFox_detection.py
index caf69fb6bc5d6c7426e2e76d82c5cb5107450d86..6b64c32833302488bc7364f3935be6429bd641cd 100755
--- a/src/scripts/tensorflow_hw_mvBlueFox_detection.py
+++ b/src/scripts/tensorflow_hw_mvBlueFox_detection.py
@@ -29,11 +29,11 @@ from stuff.helper import FPS2, WebcamVideoStream, SessionWorker
import time
import sys
-sys.path.insert(0, '/home/nvidia/catkin_ws/src/uav_hw_camera/src/scripts/')
+sys.path.insert(0, '/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/')
## LOAD CONFIG PARAMS ##
-if (os.path.isfile('/home/nvidia/catkin_ws/src/uav_hw_camera/src/scripts/config.yml')):
- with open("/home/nvidia/catkin_ws/src/uav_hw_camera/src/scripts/config.yml", 'r') as ymlfile:
+if (os.path.isfile('/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/config.yml')):
+ with open("/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/config.yml", 'r') as ymlfile:
cfg = yaml.load(ymlfile)
else:
with open("config.sample.yml", 'r') as ymlfile:
@@ -67,7 +67,7 @@ class tensorflow_detection:
with tf.Session(graph=self.detection_graph, config=config) as self.sess:
self.cam_read = Image()
# topic where we publish
- self.image_pub = rospy.Publisher("/tensorflow_detection/image", Image, queue_size=10)
+ #self.image_pub = rospy.Publisher("/tensorflow_detection/image", Image, queue_size=10)
self.bridge = CvBridge()
# topic where the coordinates go
self.cam_pose_pub = rospy.Publisher("/tensorflow_detection/cam_point", Point, queue_size=1)
@@ -75,7 +75,7 @@ class tensorflow_detection:
self.cam_pose_center_pub = rospy.Publisher("/tensorflow_detection/cam_point_center", Point, queue_size=1)
self.cam_pose_center = Point()
# subscribed Topic
- self.subscriber = rospy.Subscriber("/mv_29901984/image_raw", Image, self.callback, queue_size=1)
+ self.subscriber = rospy.Subscriber("/mv_29901972/image_raw", Image, self.callback, queue_size=1)
#self.detection(graph, category, score, expand)
@@ -153,10 +153,10 @@ class tensorflow_detection:
self.cam_pose_center.y = float("inf")
self.cam_pose_center.z = float("inf")
self.cam_pose_center_pub.publish(self.cam_pose_center)
- try:
- self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
- except CvBridgeError as e:
- print(e)
+ #try:
+ # self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
+ #except CvBridgeError as e:
+ # print(e)
def find_relative_pose(self, x, y, w, h):
quad_3d = np.float32([[-0.345, -0.345, 0], [0.345, -0.345, 0], [0.345, 0.345, 0], [-0.345, 0.345, 0]])
quad = np.float32(
@@ -358,55 +358,5 @@ def main():
if __name__ == '__main__':
main()
-#height, width, _ = cv_image.shape
-#config = tf.ConfigProto(allow_soft_placement=True, log_device_placement=log_device)
-#config.gpu_options.allow_growth = allow_memory_growth
-#with self.graph.as_default():
-# with tf.Session(graph=self.graph, config=config) as sess:
-# # Define Input and Ouput tensors
-# image_tensor = self.graph.get_tensor_by_name('image_tensor:0')
-# detection_boxes = self.graph.get_tensor_by_name('detection_boxes:0')
-# detection_scores = self.graph.get_tensor_by_name('detection_scores:0')
-# detection_classes = self.graph.get_tensor_by_name('detection_classes:0')
-# num_detections = self.graph.get_tensor_by_name('num_detections:0')
-#
-# image = cv_image # video_stream.read()
-# image_expanded = np.expand_dims(image, axis=0)
-# boxes, scores, classes, num = sess.run(
-# [detection_boxes, detection_scores, detection_classes, num_detections],
-# feed_dict={image_tensor: image_expanded})
-# vis_util.visualize_boxes_and_labels_on_image_array(
-# cv_image,
-# np.squeeze(boxes),
-# np.squeeze(classes).astype(np.int32),
-# np.squeeze(scores),
-# self.category_index,
-# use_normalized_coordinates=True,
-# line_thickness=8)
-# for i in range(0, num_detections):
-# score = scores[0][i]
-# if score >= self.threshold:
-# ymin = boxes[0][i][0] * height
-# xmin = boxes[0][i][1] * width
-# ymax = boxes[0][i][2] * height
-# xmax = boxes[0][i][3] * width
-# centerX = xmin + ((xmax - xmin) / 2)
-# centerY = ymin + ((ymax - ymin) / 2)
-# print("center x: {}\ncenter y: {}\nwidth: {}\nheight: {}".format(centerX, centerY,
-# (xmax - xmin),
-# (ymax - ymin)))
-# self.find_relative_pose(centerX, centerY, (xmax - xmin), (ymax - ymin))
-# else:
-# self.cam_pose = Point()
-# self.cam_pose.x = float("inf")
-# self.cam_pose.y = float("inf")
-# self.cam_pose.z = float("inf")
-# self.cam_pose_pub.publish(self.cam_pose)
-# try:
-# self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
-# except CvBridgeError as e:
-# print(e)
-
-