diff --git a/src/launch/autonomous_offboard_coverage_descend.launch b/src/launch/autonomous_offboard_coverage_descend.launch
index 8be0a7c5f655c7b51fdd6d401081aea14a1e3f6a..ddb1f146ddc2602b60fe380b637cdd8d0d6723d2 100644
--- a/src/launch/autonomous_offboard_coverage_descend.launch
+++ b/src/launch/autonomous_offboard_coverage_descend.launch
@@ -3,7 +3,7 @@
<node name="action_controller_coverage_descend" pkg="autonomous_offboard" type="action_controller_coverage_descend.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="tensorflow_detection" pkg="autonomous_offboard" type="tensorflow_hw_mvBlueFox_detection.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" />
+ <node name="coverage_server" pkg="autonomous_offboard" type="coverage_server.py" output="screen" />
+ <!--node name="tensorflow_detection" pkg="autonomous_offboard" type="tensorflow_hw_mvBlueFox_detection.py" output="screen" /-->
+ <!--node name="descend_on_object_server" pkg="autonomous_offboard" type="descend_on_object_server.py" output="screen" /-->
</launch>
diff --git a/src/launch/autonomous_offboard_gotopos_sensor.launch b/src/launch/autonomous_offboard_gotopos_sensor.launch
index 7a9b4f948b1c3a806d3986a499a750b3ddadb52b..67aedbb85fdd2efcdb1743c43ed27ac4b882f7a6 100644
--- a/src/launch/autonomous_offboard_gotopos_sensor.launch
+++ b/src/launch/autonomous_offboard_gotopos_sensor.launch
@@ -1,6 +1,6 @@
<launch>
<node name="position_control" pkg="autonomous_offboard" type="position_control.py" output="screen" />
<node name="action_controllerGotopos_sensor" pkg="autonomous_offboard" type="action_controllerGotopos_sensor.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="gotoposition_sensor_server" pkg="autonomous_offboard" type="gotoposition_sensor_server.py" output="screen" />
</launch>
diff --git a/src/scripts/action_controllerGotopos_sensor.py b/src/scripts/action_controllerGotopos_sensor.py
index 5a7ee9638bf0b2deb5c3089596e91c41162e8890..272f88e989c96695a810e69e16b7f74b534828dd 100755
--- a/src/scripts/action_controllerGotopos_sensor.py
+++ b/src/scripts/action_controllerGotopos_sensor.py
@@ -6,17 +6,25 @@ import actionlib
import mavros_state
import time
+import gripper_control
+
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, gotoposition_sensorAction , gotoposition_sensorGoal
-from std_msgs.msg import Float32
+from std_msgs.msg import Float32, UInt16
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
rospy.init_node('action_controllerGotopos_sensor')
+ gc = gripper_control.gripper_control()
gotoposition_sensor_client = actionlib.SimpleActionClient('gotoposition_sensor', gotoposition_sensorAction)
gotoposition_sensor_client.wait_for_server()
gotoposition_sensor_goal = gotoposition_sensorGoal()
+
+ 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()
+ msg = UInt16()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
@@ -24,14 +32,70 @@ if __name__ == '__main__':
rospy.loginfo("Taking off")
time.sleep(10)
- rospy.loginfo("First position")
- gotoposition_sensor_goal.destination.pose.position.x = 2
+ rospy.loginfo("Detect position")
+ gotoposition_sensor_goal.destination.pose.position.x = 20.5
+ gotoposition_sensor_goal.destination.pose.position.y = 45
+ gotoposition_sensor_goal.destination.pose.position.z = 5
+ gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+ gotoposition_sensor_client.wait_for_result()
+ #DESCENDING
+ rospy.loginfo("closing grippers")
+ msg.data = 180
+ gc.close_short_gripper(msg)
+
+
+ time.sleep(3)
+
+ rospy.loginfo("Drop off position")
+ gotoposition_sensor_goal.destination.pose.position.x = 100
+ gotoposition_sensor_goal.destination.pose.position.y = -27
+ gotoposition_sensor_goal.destination.pose.position.z = 5
+ gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+ gotoposition_sensor_client.wait_for_result()
+
+ time.sleep(3)
+
+ rospy.loginfo("going down to drop off position")
+ gotoposition_sensor_goal.destination.pose.position.x = 100
+ gotoposition_sensor_goal.destination.pose.position.y = -27
+ gotoposition_sensor_goal.destination.pose.position.z = 0.25
+ gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+ gotoposition_sensor_client.wait_for_result()
+
+ rospy.loginfo("opening grippers")
+ gc.open_short_gripper(msg)
+ time.sleep(2)
+ gc.Goff_gripper(msg)
+ time.sleep(3)
+
+ rospy.loginfo("Going to up before returning home")
+ gotoposition_sensor_goal.destination.pose.position.x = 100
+ gotoposition_sensor_goal.destination.pose.position.y = -27
+ gotoposition_sensor_goal.destination.pose.position.z = 5
+ gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+ gotoposition_sensor_client.wait_for_result()
+
+ time.sleep(3)
+
+ rospy.loginfo("returning home")
+ gotoposition_sensor_goal.destination.pose.position.x = 0
gotoposition_sensor_goal.destination.pose.position.y = 0
- gotoposition_sensor_goal.destination.pose.position.z = 3
+ gotoposition_sensor_goal.destination.pose.position.z = 5
gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
gotoposition_sensor_client.wait_for_result()
+ time.sleep(3)
+
+ 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.15
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
+ goto_position_vel_client.wait_for_result()
+ rospy.loginfo("Trying to land, 2 second sleep")
+ time.sleep(2)
+
+ mv_state.arm(False)
+
- time.sleep(10)
- mv_state.land(0.0)
diff --git a/src/scripts/action_controller_coverage_descend.py b/src/scripts/action_controller_coverage_descend.py
index 2709bb59d07ea92927bc2f9d4f65addc6ea4ea8e..4558fd851b9764edac1f7bd80ea32edb79bed342 100755
--- a/src/scripts/action_controller_coverage_descend.py
+++ b/src/scripts/action_controller_coverage_descend.py
@@ -24,12 +24,12 @@ if __name__ == '__main__':
mv_state.arm(True)
rospy.loginfo("Taking off")
time.sleep(10)
-
+ rospy.loginfo("going to coverage")
coverage_client = actionlib.SimpleActionClient('coverage', coverageAction)
coverage_client.wait_for_server()
coverage_goal = coverageGoal()
- coverage_goal.destination.pose.position.x = 5
- coverage_goal.destination.pose.position.y = 0
+ coverage_goal.destination.pose.position.x = -10
+ coverage_goal.destination.pose.position.y = 20
coverage_goal.destination.pose.position.z = 5
coverage_client.send_goal(coverage_goal)
coverage_client.wait_for_result()
@@ -39,69 +39,14 @@ if __name__ == '__main__':
res.pose.position.x,
res.pose.position.y,
res.pose.position.z))
- if res.pose.position.x == float("inf"):
- rospy.loginfo("ABORT no object detected. going home")
- mv_state.land(0.0)
- mv_state.arm(False)
- else:
- rospy.loginfo("Going to detected position")
- goto_position_vel_goal.destination.pose.position.x = coverage_client.get_result().detected_object_pos.pose.position.x
- goto_position_vel_goal.destination.pose.position.y = coverage_client.get_result().detected_object_pos.pose.position.y
- goto_position_vel_goal.destination.pose.position.z = coverage_client.get_result().detected_object_pos.pose.position.z
- goto_position_vel_client.send_goal(goto_position_goal)
- goto_position_vel_client.wait_for_result()
- rospy.loginfo("Is at position (x,y,z)=({}, {}, {})".format(coverage_client.get_result().detected_object_pos.pose.position.x,
- coverage_client.get_result().detected_object_pos.pose.position.y,
- coverage_client.get_result().detected_object_pos.pose.position.z))
-
- rospy.loginfo("Descending on object")
- descend_on_object_client = actionlib.SimpleActionClient('descend_on_object', descend_on_objectAction)
- descend_on_object_client.wait_for_server()
- rospy.loginfo("Descending server started")
- descend_on_object_goal = descend_on_objectGoal()
- descend_on_objectGoal = 2.0
- descend_on_object_client.send_goal(descend_on_object_goal)
- descend_on_object_client.wait_for_result()
- if descend_on_object_client.get_result().position_reached.data:
- print("landing")
- mv_state.arm(False)
- 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(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)
+ rospy.loginfo("ABORT no object detected. going home")
+ 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, 2 second sleep")
+ time.sleep(2)
+
+ mv_state.arm(False)
diff --git a/src/scripts/action_controller_descend.py b/src/scripts/action_controller_descend.py
index 64d53dadc394f7190e0394c5bc9673e6c72f8d45..5fac8ab740fc439214c510f010655b294fb71124 100755
--- a/src/scripts/action_controller_descend.py
+++ b/src/scripts/action_controller_descend.py
@@ -13,33 +13,33 @@ from std_msgs.msg import Float32
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
- rospy.init_node('action_controller_descend')
+ rospy.init_node('action_coverage_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()
+ gotoposition_sensor_client = actionlib.SimpleActionClient('gotoposition_sensor', gotoposition_sensorAction)
+ gotoposition_sensor_client.wait_for_server()
+ gotoposition_sensor_goal = gotoposition_sensorGoal()
+ descend_on_object_client = actionlib.SimpleActionClient('descend_on_object', descend_on_objectAction)
+ descend_on_object_client.wait_for_server()
+ descend_on_object_goal = descend_on_objectGoal()
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
- print 'Takeoff'
mv_state.arm(True)
- print 'Sleeping 20 sec'
- time.sleep(20)
+ rospy.loginfo("Taking off")
+ time.sleep(10)
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()
+ gotoposition_sensor_goal.destination.pose.position.x = 20.5
+ gotoposition_sensor_goal.destination.pose.position.y = 45
+ gotoposition_sensor_goal.destination.pose.position.z = 5
+ gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+ gotoposition_sensor_client.wait_for_result()
- time.sleep(2)
-
rospy.loginfo("Descending on object")
- descend_on_object_client = actionlib.SimpleActionClient('descend_on_object', descend_on_objectAction)
- descend_on_object_client.wait_for_server()
rospy.loginfo("Descending server started")
- descend_on_object_goal = descend_on_objectGoal()
descend_on_objectGoal = 2.0
descend_on_object_client.send_goal(descend_on_object_goal)
descend_on_object_client.wait_for_result()
@@ -49,40 +49,37 @@ 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(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)
+ time.sleep(3)
+
+ print 'Setting offboard'
+ mv_state.set_mode('OFFBOARD')
+ print 'Arming vehicle'
+ mv_state.arm(True)
+
+ time.sleep(3)
+
+ rospy.loginfo("Get some height")
+ 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 = 5
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
+ goto_position_client.wait_for_result()
+
+ time.sleep(3)
+ rospy.loginfo("Going home")
+ gotoposition_sensor_goal.destination.pose.position.x = 0
+ gotoposition_sensor_goal.destination.pose.position.y = 0
+ gotoposition_sensor_goal.destination.pose.position.z = 5
+ gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+ gotoposition_sensor_client.wait_for_result()
+
+ time.sleep(3)
+
+ rospy.loginfo("Get some height")
+ 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.25
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
+ goto_position_client.wait_for_result()
+
+
diff --git a/src/scripts/action_controller_pid_square.py b/src/scripts/action_controller_pid_square.py
index c66f2c606440df9dbacf3cb71f91e7d93135d245..c7a147e2756ecf4996cb803d3cf3b558c26f8cb3 100755
--- a/src/scripts/action_controller_pid_square.py
+++ b/src/scripts/action_controller_pid_square.py
@@ -25,7 +25,7 @@ if __name__ == '__main__':
time.sleep(10)
rospy.loginfo("First position")
- goto_position_vel_goal.destination.pose.position.x = 5
+ 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)
@@ -33,17 +33,17 @@ if __name__ == '__main__':
time.sleep(10)
- rospy.loginfo("Second position")
- goto_position_vel_goal.destination.pose.position.x = 0
- goto_position_vel_goal.destination.pose.position.y = -5
+ rospy.loginfo("First position")
+ goto_position_vel_goal.destination.pose.position.x = 10
+ 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)
- rospy.loginfo("Third position")
- goto_position_vel_goal.destination.pose.position.x = -5
+ rospy.loginfo("Second 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 = 3
goto_position_vel_client.send_goal(goto_position_vel_goal)
@@ -51,13 +51,13 @@ if __name__ == '__main__':
time.sleep(10)
- rospy.loginfo("Fourth position")
+ rospy.loginfo("Landing")
goto_position_vel_goal.destination.pose.position.x = 0
- goto_position_vel_goal.destination.pose.position.y = 5
- goto_position_vel_goal.destination.pose.position.z = 3
+ goto_position_vel_goal.destination.pose.position.y = 0
+ goto_position_vel_goal.destination.pose.position.z = -0.15
goto_position_vel_client.send_goal(goto_position_vel_goal)
goto_position_vel_client.wait_for_result()
+ rospy.loginfo("Trying to land, 2 second sleep")
+ time.sleep(2)
- time.sleep(10)
-
- mv_state.land(0.0)
+ mv_state.arm(False)
diff --git a/src/scripts/action_controller_takeoff_test.py b/src/scripts/action_controller_takeoff_test.py
index 69c8c0aaabba1dfb5656324bc1f251eb4140d1ef..9f6c86e3c802a527eb94428bb3a8949ea8d4fb9b 100755
--- a/src/scripts/action_controller_takeoff_test.py
+++ b/src/scripts/action_controller_takeoff_test.py
@@ -28,20 +28,20 @@ if __name__ == '__main__':
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_goal.destination.pose.position.y = 5
+ goto_position_vel_goal.destination.pose.position.z = 5
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)
+ #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, 2 second sleep")
+ #time.sleep(2)
- mv_state.arm(False)
+ #mv_state.arm(False)
diff --git a/src/scripts/coverage_server.py b/src/scripts/coverage_server.py
index 642bd15903077120938af697a1557cc6bd29a2b1..a481137cb4b24f39929c36d056b583a028ee298d 100755
--- a/src/scripts/coverage_server.py
+++ b/src/scripts/coverage_server.py
@@ -17,11 +17,11 @@ class coverage_server():
self.cur_pose = PoseStamped()
self.waypoints = np.empty((0,3))
self.destination = PoseStamped()
- self.angleOfViewWidth = 37
- self.angleOfViewHeight = 47
- self.areaWidth = 20
- self.areaHeight = 20
- self.searchHeight = 10
+ self.angleOfViewWidth = 37.0
+ self.angleOfViewHeight = 47.0
+ self.areaWidth = 20.0
+ self.areaHeight = 20.0
+ self.searchHeight = 10.0
self.detectedPos = PoseStamped()
self.detectedPos.pose.position.x = float("inf")
self.result = autonomous_offboard.msg.coverageResult()
@@ -29,8 +29,9 @@ class coverage_server():
rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.cur_pose_cb)
self.set_xy_vel = rospy.Publisher('/position_control/set_xy_vel', Float32, queue_size=10)
+ self.position_publisher = rospy.Publisher('/coverage_server/position', PoseStamped, queue_size=10)
- self.goto_position_client = actionlib.SimpleActionClient('goto_position', goto_position_velAction)
+ self.goto_position_client = actionlib.SimpleActionClient('goto_position_vel', goto_position_velAction)
#self.detect_object_client = actionlib.SimpleActionClient('detect_object', detect_objectAction)
self.rate = rospy.Rate(20)
self.action_server = actionlib.SimpleActionServer('coverage',
@@ -43,6 +44,8 @@ class coverage_server():
xyVel.data = 1
self.set_xy_vel.publish(xyVel)
goto_position_goal = goto_position_velGoal()
+ goto_position_goal.destination.pose.orientation.w = 0.707
+ goto_position_goal.destination.pose.orientation.z = 0.707
self.goto_position_client.wait_for_server()
#self.detect_object_client.wait_for_server()
self.destination = goal.destination
@@ -52,6 +55,7 @@ class coverage_server():
goto_position_goal.destination.pose.position.x = self.waypoints.item((idx, 0))
goto_position_goal.destination.pose.position.y = self.waypoints.item((idx, 1))
goto_position_goal.destination.pose.position.z = self.waypoints.item((idx, 2))
+ self.position_publisher.publish(self.cur_pose)
self.goto_position_client.send_goal(goto_position_goal)
self.goto_position_client.wait_for_result()
idx = idx + 1
@@ -73,22 +77,22 @@ class coverage_server():
#calculate waypoints
def calculate_waypoints(self):
- cameraWidth = 2 * self.searchHeight * math.tan(math.radians(self.angleOfViewWidth/2))
- cameraHeight = 2 * self.searchHeight * math.tan(math.radians(self.angleOfViewHeight/2))
+ cameraWidth = 2 * self.searchHeight * math.tan(math.radians(self.angleOfViewWidth/2.0))
+ cameraHeight = 2 * self.searchHeight * math.tan(math.radians(self.angleOfViewHeight/2.0))
lastWaypoint = PoseStamped()
- lastWaypoint.pose.position.x = self.destination.pose.position.x - self.areaWidth/2
- lastWaypoint.pose.position.y = self.destination.pose.position.y - self.areaHeight / 2
+ lastWaypoint.pose.position.x = self.destination.pose.position.x - self.areaWidth/2.0
+ lastWaypoint.pose.position.y = self.destination.pose.position.y - self.areaHeight / 2.0
lastWaypoint.pose.position.z = self.searchHeight
tempArr = []
tempArr.append([lastWaypoint.pose.position.x, lastWaypoint.pose.position.y, lastWaypoint.pose.position.z])
- while(lastWaypoint.pose.position.x + cameraWidth/2 < self.destination.pose.position.x + self.areaWidth/2):
+ while(lastWaypoint.pose.position.x + cameraWidth/2.0 < self.destination.pose.position.x + self.areaWidth/2.0):
#check if going upwards or downwards
if(cameraHeight > 0):
- while(lastWaypoint.pose.position.y + cameraHeight/2 < self.destination.pose.position.y + self.areaHeight/2):
+ while(lastWaypoint.pose.position.y + cameraHeight/2.0 < self.destination.pose.position.y + self.areaHeight/2.0):
lastWaypoint.pose.position.y = lastWaypoint.pose.position.y + cameraHeight
tempArr.append([lastWaypoint.pose.position.x, lastWaypoint.pose.position.y, lastWaypoint.pose.position.z])
elif(cameraHeight < 0):
- while (lastWaypoint.pose.position.y + cameraHeight / 2 > self.destination.pose.position.y - self.areaHeight / 2):
+ while (lastWaypoint.pose.position.y + cameraHeight / 2.0 > self.destination.pose.position.y - self.areaHeight / 2.0):
lastWaypoint.pose.position.y = lastWaypoint.pose.position.y + cameraHeight
tempArr.append([lastWaypoint.pose.position.x, lastWaypoint.pose.position.y, lastWaypoint.pose.position.z])
@@ -101,10 +105,10 @@ class coverage_server():
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.cameraWidth = 2.0 * self.searchHeight * math.tan(math.radians(self.angleOfViewWidth / 2.0))
+ self.cameraHeight = 2.0 * self.searchHeight * math.tan(math.radians(self.angleOfViewHeight / 2.0))
+ startPointX = self.destination.pose.position.x - self.areaWidth / 2.0 + self.cameraWidth / 2.0
+ startPointY = self.destination.pose.position.y - self.areaHeight / 2.0 + self.cameraHeight / 2.0
self.nrOfStepsHeight = int(math.floor(self.areaHeight / self.cameraHeight))
self.nrOfStepsWidth = int(math.floor(self.areaWidth / self.cameraWidth))
diff --git a/src/scripts/descend_on_object_server.py b/src/scripts/descend_on_object_server.py
index dfdff49034672000f6b3ff0a4e83727ade5fca79..9ee711a7d7a67690400d6bed16236a5c3caea5b4 100755
--- a/src/scripts/descend_on_object_server.py
+++ b/src/scripts/descend_on_object_server.py
@@ -2,6 +2,8 @@
import rospy
import actionlib
import autonomous_offboard.msg
+import tf
+import math
from geometry_msgs.msg import PoseStamped, Point
from std_msgs.msg import Bool, String, Float32
import time
@@ -13,6 +15,11 @@ class descend_on_object_server():
# variables
self.local_pose = PoseStamped()
self.des_pose = PoseStamped()
+ quaternion = tf.transformations.quaternion_from_euler(0, 0, math.radians(90))
+ des_pose.pose.orientation.x = quaternion[0]
+ des_pose.pose.orientation.y = quaternion[1]
+ des_pose.pose.orientation.z = quaternion[2]
+ des_pose.pose.orientation.w = quaternion[3]
self.object_pose = Point()
self.object_pose_center = Point()
@@ -102,7 +109,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 = -0.1
+ self.des_pose.pose.position.z = -0.15
self.vel_control.publish(self.des_pose)
time.sleep(0.1)
while not self.target_reached:
diff --git a/src/scripts/goto_position_vel_server.py b/src/scripts/goto_position_vel_server.py
index 02b51f5290ac538f4540944346e94850e8be4b35..471801ebd834f815207c354bfe48d78fec17e3ad 100755
--- a/src/scripts/goto_position_vel_server.py
+++ b/src/scripts/goto_position_vel_server.py
@@ -2,7 +2,7 @@
import rospy
import actionlib
import autonomous_offboard.msg
-from std_msgs.msg import Bool, String
+from std_msgs.msg import Bool, String, Float32
from geometry_msgs.msg import PoseStamped
class goto_position_vel_server():
@@ -13,7 +13,6 @@ class goto_position_vel_server():
#publishers
self.pose_control = rospy.Publisher('/position_control/set_velocity', PoseStamped, queue_size=10)
self.mode_control = rospy.Publisher('/position_control/set_mode', String, queue_size=10)
-
#subscribers
rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
diff --git a/src/scripts/gotoposition_sensor_server.py b/src/scripts/gotoposition_sensor_server.py
index 213939b31f8be43e17389a6bd47c18d97ca59ea1..8d333c3f8c0b892d06f0a88a118425a4d10dcc2d 100755
--- a/src/scripts/gotoposition_sensor_server.py
+++ b/src/scripts/gotoposition_sensor_server.py
@@ -23,6 +23,7 @@ class gotoposition_sensor_server():
self.avoid_pose = PoseStamped()
self.goal_pose = PoseStamped()
self.distance = Float32()
+ self.target_reached = False
self.yaw = Float32()
@@ -32,12 +33,12 @@ class gotoposition_sensor_server():
self.vel_control = rospy.Publisher('/position_control/set_velocity', PoseStamped, queue_size=10)
self.velpose_control = rospy.Publisher('/position_control/set_velocityPose',PoseStamped,queue_size=10)
+
# ---------------Subscribers---------------#
rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
rospy.Subscriber('/position_control/Real_pose', PoseStamped, self._Real_pose_callback)
rospy.Subscriber('/position_control/set_mode', String, self.set_mode_callback)
- rospy.Subscriber('/position_control/Yawangle', Float32, self.yaw_callback)
# ---------------Start---------------#
self.rate = rospy.Rate(20)
@@ -46,19 +47,16 @@ class gotoposition_sensor_server():
self.action_server.start()
def execute_cb(self, goal):
-
#Set the orientation before start in position mode.
self.mode_control.publish('velposctr')
- Azimut = math.atan2(goal.destination.pose.position.y - self.Real_pose.pose.position.y,
- goal.destination.pose.position.x - self.Real_pose.pose.position.x)
- quaternion = tf.transformations.quaternion_from_euler(0, 0, Azimut)
+ quaternion = tf.transformations.quaternion_from_euler(0, 0, math.radians(90))
goal.destination.pose.orientation.x = quaternion[0]
goal.destination.pose.orientation.y = quaternion[1]
goal.destination.pose.orientation.z = quaternion[2]
goal.destination.pose.orientation.w = quaternion[3]
self.goal_pose = goal.destination
self.velpose_control.publish(goal.destination)
-
+ rospy.sleep(0.1)
while not self.target_reached:
self.rate.sleep()
@@ -79,8 +77,7 @@ class gotoposition_sensor_server():
self.current_mode = data
- def yaw_callback(self,data):
- self.yaw = data
+
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 9077a968d1d9f2691d5b4023c29b100619ec08c8..c1f3353866dd7115eb95618502011526e62a020a 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 32eb4cced771161d71020187613a112f0c141aef..e5df4e7b814fc0e6848f5756377893de99f93eb0 100755
--- a/src/scripts/position_control.py
+++ b/src/scripts/position_control.py
@@ -223,11 +223,16 @@ class position_control():
booldist = self.is_at_position(self.local_pose.pose.position, self._pose_msg.pose.position, 0.5)
boolvel = self.hover_velocity()
self.dist.publish(booldist and boolvel)
- elif self.current_mode.data == 'velctr' or self.current_mode.data == 'velposctr':
+ elif self.current_mode.data == 'velctr':
# print("target vel_pos: {}".format(vel_pose_tot))
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)
+ elif self.current_mode.data == 'velposctr':
+ # print("target vel_pos: {}".format(vel_pose_tot))
+ booldist = self.is_at_position(self.real_pose.pose.position, self._velpose_msg.pose.position, 0.3)
+ boolvel = self.hover_velocity()
+ self.dist.publish(booldist and boolvel)
def is_at_position(self, p_current, p_desired, offset):
des_pos = np.array((p_desired.x,
diff --git a/src/scripts/tensorflow_hw_mvBlueFox_detection.py b/src/scripts/tensorflow_hw_mvBlueFox_detection.py
index 6b64c32833302488bc7364f3935be6429bd641cd..5cac0d2c5b13d40bb7e0fd00f05705fb2d0d76e6 100755
--- a/src/scripts/tensorflow_hw_mvBlueFox_detection.py
+++ b/src/scripts/tensorflow_hw_mvBlueFox_detection.py
@@ -6,8 +6,8 @@ import rospy
# Ros Messages
from sensor_msgs.msg import CompressedImage, Image
from cv_bridge import CvBridge, CvBridgeError
-from geometry_msgs.msg import Point
-
+from geometry_msgs.msg import Point, PoseStamped
+from std_msgs.msg import Bool
#Tensorflow imports
import numpy as np
import os
@@ -29,7 +29,7 @@ from stuff.helper import FPS2, WebcamVideoStream, SessionWorker
import time
import sys
-sys.path.insert(0, '/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/')
+sys.path.insert(0, '/home/pateman/catkin_ws/src/autonomous_offboard/src/scripts')
## LOAD CONFIG PARAMS ##
if (os.path.isfile('/home/nvidia/catkin_ws/src/autonomous_offboard/src/scripts/config.yml')):
@@ -58,16 +58,33 @@ ssd_shape = cfg['ssd_shape']
class tensorflow_detection:
def __init__(self):
+ self.quad_3d_frame = np.float32([[-0.345, -0.345, 0], [0.345, -0.345, 0], [0.345, 0.345, 0], [-0.345, 0.345, 0]])
+ self.quad_3d_center = np.float32([[-0.11075, -0.11075, 0], [0.11075, -0.11075, 0], [0.11075, 0.11075, 0], [-0.11075, 0.11075, 0]])
+ #----REAL---
+ self.K = np.float64([[1168.084098028661, 0, 545.2679414486556],
+ [0, 1164.433590234314, 526.4386170901346],
+ [0.0, 0.0, 1.0]])
+ # ----GAZEBO---
+ #self.K = np.float64([[1472.512772, 0, 640.5],
+ # [0, 1472.512772, 480.5],
+ # [0.0, 0.0, 1.0]])
+ self.dist_coef = np.float64([-0.06624560142145453, 0.05966170895627322, 0.002933236970742761, -0.01336570576300551, 0])
self.detection_graph, score, expand = self.load_frozenmodel()
self.category_index = self.load_labelmap()
config = tf.ConfigProto(allow_soft_placement=True, log_device_placement=log_device)
config.gpu_options.allow_growth = allow_memory_growth
+ self.target_reached = False
+ self.local_pose = PoseStamped()
cur_frames = 0
+ self.write_flag = False
+ self.write_flag_center = False
+ self.countDet = 0
+ self.countDet_center = 0
with self.detection_graph.as_default():
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)
@@ -76,14 +93,21 @@ class tensorflow_detection:
self.cam_pose_center = Point()
# subscribed Topic
self.subscriber = rospy.Subscriber("/mv_29901972/image_raw", Image, self.callback, queue_size=1)
-
- #self.detection(graph, category, score, expand)
+ #self.subscriber = rospy.Subscriber("/uav_camera/image_raw_down", Image, self.callback, queue_size=1)
+ #rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
+ #rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
+ #self.positioning_file = open("/home/pateman/catkin_ws/src/autonomous_offboard/src/positioning_data.txt",
+ # "w")
self.threshold = 0.6
-
+ def distance_reached_cb(self, data):
+ self.target_reached = data.data
+ def _local_pose_callback(self, data):
+ self.local_pose = data
def callback(self, ros_data):
+
try:
cv_image = self.bridge.imgmsg_to_cv2(ros_data, "bgr8")
except CvBridgeError as e:
@@ -114,7 +138,10 @@ class tensorflow_detection:
for i in range(0, num):
score = scores[0][i]
if score >= self.threshold:
- print(int(classes[0][i]))
+ #if (self.target_reached == False):
+ # self.write_flag = False
+ # self.write_flag_center = False
+ #print(int(classes[0][i]))
#if the whole frame is detected
if int(classes[0][i]) == 1:
frame_flag_detected = True
@@ -153,22 +180,18 @@ 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(
- [[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
-
- K = np.float64([[1472.512772, 0, 640.5],
- [0, 1472.512772, 480.5],
- [0.0, 0.0, 1.0]])
-
-
- dist_coef = np.zeros(4)
- _ret, rvec, tvec = cv2.solvePnP(quad_3d, quad, K, dist_coef)
+ #-----GAZEBO-----#
+ #quad = np.float32(
+ # [[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
+ #-------REAL--------#
+ quad = np.float32([[x - w / 2, y - h / 2], [x + w / 2, y - h / 2], [x + w / 2, y + h / 2], [x - w / 2, y + h / 2]])
+ #dist_coef_zero = np.zeros(4)
+ _ret, rvec, tvec = cv2.solvePnP(self.quad_3d_frame, quad, self.K, self.dist_coef)
rmat = cv2.Rodrigues(rvec)[0]
#self.rotationMatrixToEulerAngles(rmat)
cameraTranslatevector = -np.matrix(rmat).T * np.matrix(tvec)
@@ -177,26 +200,32 @@ class tensorflow_detection:
T0[:3, :3] = rmat
T0[:4, 3] = [0, 0, 0, 1]
T0[:3, 3] = np.transpose(cameraTranslatevector)
-
- p0 = np.array([-0.345 / 2, -0.3011 / 2, 0, 1])
+ ####----REAL----####
+ p0 = np.array([0.300 / 2, -0.345 / 2, 0, 1])
+ ###-----GAZEBO----#
+ #p0 = np.array([-0.345 / 2, -0.345 / 2, 0, 1])
z0 = np.dot(T0, p0)
- self.cam_pose.x = z0.item(0)
+ self.cam_pose.x = -z0.item(0)
self.cam_pose.y = z0.item(1)
self.cam_pose.z = z0.item(2)
+ #if (self.target_reached and self.write_flag == False):
+ # self.positioning_file.write(
+ # "Frame: Local position: [{}, {}, {}], Object position: [5, 5, 0], positioning: [{}, {}, {}], count: {}\n".format(
+ # self.local_pose.pose.position.x, self.local_pose.pose.position.y, self.local_pose.pose.position.z,
+ # self.cam_pose.x, self.cam_pose.y, self.cam_pose.z, self.countDet))
+ # self.write_flag = True
+ # self.countDet += 1
self.cam_pose_pub.publish(self.cam_pose)
def find_relative_pose_center(self, x, y, w, h):
- quad_3d = np.float32([[-0.11075, -0.11075, 0], [0.11075, -0.11075, 0], [0.11075, 0.11075, 0], [-0.11075, 0.11075, 0]])
+ # -------GAZEBO--------#
+ #quad = np.float32(
+ # [[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
+ # -------REAL--------#
quad = np.float32(
- [[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
-
- K = np.float64([[1472.512772, 0, 640.5],
- [0, 1472.512772, 480.5],
- [0.0, 0.0, 1.0]])
-
-
- dist_coef = np.zeros(4)
- _ret, rvec, tvec = cv2.solvePnP(quad_3d, quad, K, dist_coef)
+ [[x - w / 2, y - h / 2], [x + w / 2, y - h / 2], [x + w / 2, y + h / 2], [x - w / 2, y + h / 2]])
+ #dist_coef_zero = np.zeros(4)
+ _ret, rvec, tvec = cv2.solvePnP(self.quad_3d_center, quad, self.K, self.dist_coef)
rmat = cv2.Rodrigues(rvec)[0]
#self.rotationMatrixToEulerAngles(rmat)
cameraTranslatevector = -np.matrix(rmat).T * np.matrix(tvec)
@@ -205,13 +234,22 @@ class tensorflow_detection:
T0[:3, :3] = rmat
T0[:4, 3] = [0, 0, 0, 1]
T0[:3, 3] = np.transpose(cameraTranslatevector)
-
- p0 = np.array([-0.2215 / 2, -0.2061 / 2, 0, 1])
+ ####----REAL----####
+ p0 = np.array([0.200 / 2, -0.100 / 2, 0, 1])
+ ####----GAZEBO----####
+ #p0 = np.array([-0.11075 / 2, -0.11075 / 2, 0, 1])
z0 = np.dot(T0, p0)
- self.cam_pose_center.x = z0.item(0)
+ self.cam_pose_center.x = -z0.item(0)
self.cam_pose_center.y = z0.item(1)
self.cam_pose_center.z = z0.item(2)
+ #if (self.target_reached and self.write_flag_center == False):
+ # self.positioning_file.write(
+ # "Center: Local position: [{}, {}, {}], Object position: [5, 5, 0], positioning: [{}, {}, {}], count: {}\n".format(
+ # self.local_pose.pose.position.x, self.local_pose.pose.position.y, self.local_pose.pose.position.z,
+ # self.cam_pose.x, self.cam_pose.y, self.cam_pose.z, self.countDet_center))
+ # self.write_flag_center = True
+ # self.countDet_center += 1
self.cam_pose_center_pub.publish(self.cam_pose_center)
def isRotationMatrix(self,R):
@@ -360,3 +398,4 @@ if __name__ == '__main__':
+