minor

action/gotoposition_sensor.action

+#goal definition
+geometry_msgs/PoseStamped destination
+---
+#result definition
+std_msgs/String data
+---
+#feedback
+geometry_msgs/PoseStamped current_position

src/launch/autonomous_offboard_coverage_descend.launch

+<launch>
+  <node name="position_control" pkg="autonomous_offboard" type="position_control.py" output="screen" />
+  <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" />
+</launch>

src/launch/autonomous_offboard_gotopos_sensor.launch

+<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="gotoposition_sensor_server" pkg="autonomous_offboard" type="gotoposition_sensor_server.py" output="screen" />
+</launch>

src/launch/autonomous_offboard_vel_square.launch

+<launch>
+  <node name="position_control" pkg="autonomous_offboard" type="position_control.py" output="screen" />
+  <node name="action_controller_pid_square" pkg="autonomous_offboard" type="action_controller_pid_square.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" />
+</launch>

src/scripts/action_controllerGotopos_sensor.py

+#!/usr/bin/env python
+import roslib
+#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_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 geometry_msgs.msg import PoseStamped
+
+if __name__ == '__main__':
+    rospy.init_node('action_controllerGotopos_sensor')
+    gotoposition_sensor_client = actionlib.SimpleActionClient('gotoposition_sensor', gotoposition_sensorAction)
+    gotoposition_sensor_client.wait_for_server()
+    gotoposition_sensor_goal = gotoposition_sensorGoal()
+    mv_state = mavros_state.mavros_state()
+    print 'Setting offboard'
+    mv_state.set_mode('OFFBOARD')
+    print 'Arming vehicle'
+    mv_state.arm(True)
+    rospy.loginfo("Taking off")
+    time.sleep(10)
+
+    rospy.loginfo("First position")
+    gotoposition_sensor_goal.destination.pose.position.x = 2
+    gotoposition_sensor_goal.destination.pose.position.y = 0
+    gotoposition_sensor_goal.destination.pose.position.z = 3
+    gotoposition_sensor_client.send_goal(gotoposition_sensor_goal)
+    gotoposition_sensor_client.wait_for_result()
+
+
+    time.sleep(10)
+
+    mv_state.land(0.0)

src/scripts/action_controller_coverage_descend.py

+#!/usr/bin/env python
+
+import roslib
+#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_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_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()
+    mv_state = mavros_state.mavros_state()
+    print 'Setting offboard'
+    mv_state.set_mode('OFFBOARD')
+    print 'Arming vehicle'
+    mv_state.arm(True)
+    rospy.loginfo("Taking off")
+    time.sleep(10)
+
+    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.z = 5
+    coverage_client.send_goal(coverage_goal)
+    coverage_client.wait_for_result()
+    res = PoseStamped()
+    res = coverage_client.get_result().detected_object_pos
+    rospy.loginfo("Detected object is at position (x,y,z)=({}, {}, {})".format(
+        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)

src/scripts/action_controller_pid_square.py

+#!/usr/bin/env python
+import roslib
+#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_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_pid_square')
+    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'
+    mv_state.arm(True)
+    rospy.loginfo("Taking off")
+    time.sleep(10)
+
+    rospy.loginfo("First position")
+    goto_position_vel_goal.destination.pose.position.x = 5
+    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("Second position")
+    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_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
+    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("Fourth position")
+    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_client.send_goal(goto_position_vel_goal)
+    goto_position_vel_client.wait_for_result()
+
+    time.sleep(10)
+
+    mv_state.land(0.0)

src/scripts/gotoposition_sensor_server.py

+#!/usr/bin/env python
+import rospy
+import actionlib
+import math
+import tf
+import numpy
+import time
+import autonomous_offboard.msg
+from std_msgs.msg import Bool, String, Float32, Float64
+from geometry_msgs.msg import PoseStamped, Quaternion
+
+
+
+
+
+
+class gotoposition_sensor_server():
+    def __init__(self):
+
+        #---------------Variables---------------#
+
+        self.local_pose = PoseStamped()
+        self.avoid_pose = PoseStamped()
+        self.goal_pose = PoseStamped()
+        self.distance = Float32()
+        self.yaw = Float32()
+
+
+        #---------------Publishers---------------#
+        self.pose_control = rospy.Publisher('/position_control/set_position', PoseStamped, queue_size=10)
+        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.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)
+        self.action_server = actionlib.SimpleActionServer('gotoposition_sensor', autonomous_offboard.msg.gotoposition_sensorAction,
+                                                          execute_cb=self.execute_cb, auto_start=False)
+        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)
+        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)
+
+
+        while not self.target_reached:
+            self.rate.sleep()
+
+        rospy.loginfo("Destination reached")
+        self.action_server.set_succeeded()
+
+
+
+    def distance_reached_cb(self, data):
+        self.target_reached = data.data
+
+    def _Real_pose_callback(self, data):
+        self.Real_pose = data
+
+
+    def set_mode_callback(self, data):
+        self.current_mode = data
+
+
+    def yaw_callback(self,data):
+        self.yaw = data
+
+
+
+
+if __name__ == '__main__':
+    try:
+
+        rospy.init_node('gotoposition_sensor_server')
+        gotoposition_sensor_server()
+    except rospy.ROSInterruptException:
+        pass

src/scripts/object-detection.pbtxt

+item{
+	id: 1
+	name: 'UAVFrame'
+}
+item{
+	id: 2
+	name: 'UAVCenter'
+}