From 5416119a62f6a72ade6752c84eeb682646ae1c53 Mon Sep 17 00:00:00 2001
From: pateman16 <patrickkarlsson16@gmail.com>
Date: Mon, 23 Apr 2018 17:20:43 +0200
Subject: [PATCH] minor changes
---
CMakeLists.txt | 1 +
src/Startup.txt | 1 -
src/launch/autonomous_offboard_pid.launch | 2 +-
.../autonomous_offboard_takeoff_test.launch | 4 +-
src/launch/autonomous_offboard_test.launch | 3 +-
src/scripts/action_controller.py | 2 +-
src/scripts/action_controller_descend.py | 2 +-
src/scripts/action_controller_pid.py | 22 ++--
src/scripts/action_controller_takeoff_test.py | 18 +--
src/scripts/action_controller_test.py | 46 ++++----
src/scripts/coverage_server.py | 4 +-
src/scripts/descend_on_object_server.py | 4 +-
src/scripts/detect_object_server.py | 4 +-
src/scripts/goto_position_server.py | 4 +-
src/scripts/goto_position_vel_server.py | 11 +-
src/scripts/long_grippers_server.py | 4 +-
src/scripts/mavros_state.pyc | Bin 4650 -> 4650 bytes
src/scripts/position_control.py | 105 ++++++++----------
src/scripts/short_grippers_server.py | 4 +-
19 files changed, 110 insertions(+), 131 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index fbd9a3f..0ebecc1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -71,6 +71,7 @@ add_action_files(
FILES
goto_position.action
goto_position_vel.action
+ takeoff_curr.action
detect_object.action
center_on_object.action
descend_on_object.action
diff --git a/src/Startup.txt b/src/Startup.txt
index 8a015fb..970ba03 100644
--- a/src/Startup.txt
+++ b/src/Startup.txt
@@ -13,7 +13,6 @@ terminal3: roslaunch simulation_control simulation_control.launch
ready to fly
-
Start Rosserial with sensor controller
rosrun rosserial_python serial_node.py -port:=/dev/USB0
diff --git a/src/launch/autonomous_offboard_pid.launch b/src/launch/autonomous_offboard_pid.launch
index f6550a0..b6a471b 100644
--- a/src/launch/autonomous_offboard_pid.launch
+++ b/src/launch/autonomous_offboard_pid.launch
@@ -1,6 +1,6 @@
<launch>
<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="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" />
diff --git a/src/launch/autonomous_offboard_takeoff_test.launch b/src/launch/autonomous_offboard_takeoff_test.launch
index 060fa79..5b5f06c 100644
--- a/src/launch/autonomous_offboard_takeoff_test.launch
+++ b/src/launch/autonomous_offboard_takeoff_test.launch
@@ -1,6 +1,6 @@
<launch>
<node name="position_control" pkg="autonomous_offboard" type="position_control.py" output="screen" />
- <node name="action_controller_test" pkg="autonomous_offboard" type="action_controller_test.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="obstacle_detection_control" pkg="simulation_control" type="obstacle_detection_control.py" output="screen" />
+ <node name="takeoff_curr_server" pkg="autonomous_offboard" type="takeoff_curr_server.py" output="screen" />
</launch>
diff --git a/src/launch/autonomous_offboard_test.launch b/src/launch/autonomous_offboard_test.launch
index cd38a3e..4bfb4e2 100644
--- a/src/launch/autonomous_offboard_test.launch
+++ b/src/launch/autonomous_offboard_test.launch
@@ -1,6 +1,5 @@
<launch>
<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="action_controller_test" pkg="autonomous_offboard" type="action_controller_test.py" output="screen" />
<node name="goto_position_server" pkg="autonomous_offboard" type="goto_position_server.py" output="screen" />
- <node name="obstacle_detection_control" pkg="simulation_control" type="obstacle_detection_control.py" output="screen" />
</launch>
diff --git a/src/scripts/action_controller.py b/src/scripts/action_controller.py
index f7c0b26..a1dea2c 100755
--- a/src/scripts/action_controller.py
+++ b/src/scripts/action_controller.py
@@ -7,7 +7,7 @@ import actionlib
import mavros_state
import time
-from simulation_control.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_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
diff --git a/src/scripts/action_controller_descend.py b/src/scripts/action_controller_descend.py
index c296c71..3548b25 100755
--- a/src/scripts/action_controller_descend.py
+++ b/src/scripts/action_controller_descend.py
@@ -7,7 +7,7 @@ import actionlib
import mavros_state
import time
-from simulation_control.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_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
diff --git a/src/scripts/action_controller_pid.py b/src/scripts/action_controller_pid.py
index f548f8d..11607d0 100755
--- a/src/scripts/action_controller_pid.py
+++ b/src/scripts/action_controller_pid.py
@@ -7,13 +7,13 @@ import actionlib
import mavros_state
import time
-from simulation_control.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 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_test')
+ rospy.init_node('action_controller_pid_test')
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
@@ -30,20 +30,21 @@ if __name__ == '__main__':
rospy.loginfo("Takeoff succeded")
time.sleep(5)
+ rospy.loginfo("Z PID")
######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_positionGoal()
+ 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_goal)
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
goto_position_vel_client.wait_for_result()
time.sleep(10)
###STABALIZE
- rospy.loginfo("Second position")
+ 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
@@ -51,17 +52,18 @@ if __name__ == '__main__':
goto_position_client.wait_for_result()
time.sleep(5)
+ rospy.loginfo("X PID")
#######X PID##############
goto_position_vel_goal.destination.pose.position.x = 1
goto_position_vel_goal.destination.pose.position.y = 0
goto_position_vel_goal.destination.pose.position.z = 2
- goto_position_vel_client.send_goal(goto_position_goal)
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
goto_position_vel_client.wait_for_result()
time.sleep(10)
###STABALIZE
- rospy.loginfo("Second position")
+ 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
@@ -71,16 +73,17 @@ if __name__ == '__main__':
time.sleep(5)
#######Y PID##############
+ rospy.loginfo("Y PID")
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
- goto_position_vel_client.send_goal(goto_position_goal)
+ goto_position_vel_client.send_goal(goto_position_vel_goal)
goto_position_vel_client.wait_for_result()
time.sleep(10)
###STABALIZE
- rospy.loginfo("Second position")
+ 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
@@ -90,3 +93,4 @@ if __name__ == '__main__':
time.sleep(5)
mv_state.land(0.0)
+
diff --git a/src/scripts/action_controller_takeoff_test.py b/src/scripts/action_controller_takeoff_test.py
index b580068..fd8d3a7 100755
--- a/src/scripts/action_controller_takeoff_test.py
+++ b/src/scripts/action_controller_takeoff_test.py
@@ -7,7 +7,7 @@ import actionlib
import mavros_state
import time
-from simulation_control.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_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal,takeoff_currAction, takeoff_currGoal, long_grippersAction, long_grippersGoal, coverageAction, coverageGoal
from std_msgs.msg import Float32
from geometry_msgs.msg import PoseStamped
@@ -18,17 +18,9 @@ if __name__ == '__main__':
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
+ print 'Takeoff'
mv_state.arm(True)
- time.sleep(5)
- 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")
-
- time.sleep(10)
-
+ print 'Sleeping 20 sec'
+ time.sleep(20)
+ print 'Landing'
mv_state.land(0.0)
diff --git a/src/scripts/action_controller_test.py b/src/scripts/action_controller_test.py
index 8c60e99..fe90c77 100755
--- a/src/scripts/action_controller_test.py
+++ b/src/scripts/action_controller_test.py
@@ -7,64 +7,58 @@ import actionlib
import mavros_state
import time
-from simulation_control.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_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_test')
+ goto_position_client = actionlib.SimpleActionClient('goto_position', goto_positionAction)
+ goto_position_client.wait_for_server()
+ goto_position_goal = goto_positionGoal()
mv_state = mavros_state.mavros_state()
print 'Setting offboard'
mv_state.set_mode('OFFBOARD')
print 'Arming vehicle'
mv_state.arm(True)
- time.sleep(5)
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")
-
- time.sleep(5)
+ time.sleep(10)
rospy.loginfo("First position")
- goto_position_goal.destination.pose.position.x = -5
+ goto_position_goal.destination.pose.position.x = 20
goto_position_goal.destination.pose.position.y = 0
- goto_position_goal.destination.pose.position.z = 2
+ 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)
+ time.sleep(10)
rospy.loginfo("Second position")
- goto_position_goal.destination.pose.position.x = -5
- goto_position_goal.destination.pose.position.y = 5
- goto_position_goal.destination.pose.position.z = 2
+ goto_position_goal.destination.pose.position.x = 0
+ goto_position_goal.destination.pose.position.y = -20
+ 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)
+ time.sleep(10)
rospy.loginfo("Third position")
- goto_position_goal.destination.pose.position.x = 0
- goto_position_goal.destination.pose.position.y = 5
- goto_position_goal.destination.pose.position.z = 2
+ goto_position_goal.destination.pose.position.x = -20
+ 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)
+ time.sleep(10)
- rospy.loginfo("Third position")
+ rospy.loginfo("Fourth position")
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_goal.destination.pose.position.y = 20
+ 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)
+ time.sleep(10)
mv_state.land(0.0)
diff --git a/src/scripts/coverage_server.py b/src/scripts/coverage_server.py
index b40ec84..68b935e 100755
--- a/src/scripts/coverage_server.py
+++ b/src/scripts/coverage_server.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
import numpy as np
import time
import math
@@ -28,7 +28,7 @@ class coverage_server():
self.detect_object_client = actionlib.SimpleActionClient('detect_object', detect_objectAction)
self.rate = rospy.Rate(20)
self.action_server = actionlib.SimpleActionServer('coverage',
- simulation_control.msg.coverageAction,
+ autonomous_offboard.msg.coverageAction,
execute_cb=self.execute_cb, auto_start=False)
self.action_server.start()
diff --git a/src/scripts/descend_on_object_server.py b/src/scripts/descend_on_object_server.py
index 1faeda3..8b92f4c 100755
--- a/src/scripts/descend_on_object_server.py
+++ b/src/scripts/descend_on_object_server.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
from geometry_msgs.msg import PoseStamped, Point
from std_msgs.msg import Bool, String
import time
@@ -29,7 +29,7 @@ class descend_on_object_server():
self.rate = rospy.Rate(20)
self.result = simulation_control.msg.descend_on_objectResult()
self.action_server = actionlib.SimpleActionServer('descend_on_object',
- simulation_control.msg.descend_on_objectAction,
+ autonomous_offboard.msg.descend_on_objectAction,
execute_cb=self.execute_cb,
auto_start=False)
self.last_object_pose = Point()
diff --git a/src/scripts/detect_object_server.py b/src/scripts/detect_object_server.py
index 7a079ed..cb6b281 100755
--- a/src/scripts/detect_object_server.py
+++ b/src/scripts/detect_object_server.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
from geometry_msgs.msg import PoseStamped, Point
class detect_object_server():
@@ -20,7 +20,7 @@ class detect_object_server():
self.rate = rospy.Rate(20)
self.result = simulation_control.msg.detect_objectResult()
- self.action_server = actionlib.SimpleActionServer('detect_object', simulation_control.msg.detect_objectAction,
+ self.action_server = actionlib.SimpleActionServer('detect_object', autonomous_offboard.msg.detect_objectAction,
execute_cb=self.execute_cb, auto_start=False)
self.action_server.start()
diff --git a/src/scripts/goto_position_server.py b/src/scripts/goto_position_server.py
index d983d65..3c7c6e5 100755
--- a/src/scripts/goto_position_server.py
+++ b/src/scripts/goto_position_server.py
@@ -1,7 +1,7 @@
#!/usr/bin/env python
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
from std_msgs.msg import Bool, String
from geometry_msgs.msg import PoseStamped
@@ -19,7 +19,7 @@ class goto_position_server():
self.rate = rospy.Rate(20)
#self.result = simulation_control.msg.goto_positionResult()
- self.action_server = actionlib.SimpleActionServer('goto_position', simulation_control.msg.goto_positionAction,
+ self.action_server = actionlib.SimpleActionServer('goto_position', autonomous_offboard.msg.goto_positionAction,
execute_cb=self.execute_cb, auto_start=False)
self.action_server.start()
diff --git a/src/scripts/goto_position_vel_server.py b/src/scripts/goto_position_vel_server.py
index bd0c42d..02b51f5 100755
--- a/src/scripts/goto_position_vel_server.py
+++ b/src/scripts/goto_position_vel_server.py
@@ -1,17 +1,17 @@
#!/usr/bin/env python
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
from std_msgs.msg import Bool, String
from geometry_msgs.msg import PoseStamped
-class goto_position_server():
+class goto_position_vel_server():
def __init__(self):
#variables
self.target_reached = False
#publishers
- self.pose_control = rospy.Publisher('/position_control/set_position', PoseStamped, queue_size=10)
+ 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
@@ -19,12 +19,13 @@ class goto_position_server():
self.rate = rospy.Rate(20)
#self.result = simulation_control.msg.goto_positionResult()
- self.action_server = actionlib.SimpleActionServer('goto_position_vel', simulation_control.msg.goto_position_velAction,
+ self.action_server = actionlib.SimpleActionServer('goto_position_vel', autonomous_offboard.msg.goto_position_velAction,
execute_cb=self.execute_cb, auto_start=False)
self.action_server.start()
def execute_cb(self, goal):
+ print(goal)
self.mode_control.publish('velctr')
self.pose_control.publish(goal.destination)
rospy.sleep(0.1)
@@ -43,6 +44,6 @@ if __name__ == '__main__':
try:
rospy.init_node('goto_position_vel_server')
- goto_position_server()
+ goto_position_vel_server()
except rospy.ROSInterruptException:
pass
diff --git a/src/scripts/long_grippers_server.py b/src/scripts/long_grippers_server.py
index 486bf4f..7e7a79f 100755
--- a/src/scripts/long_grippers_server.py
+++ b/src/scripts/long_grippers_server.py
@@ -3,7 +3,7 @@
import roslib
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
import time
from std_msgs.msg import Float32
@@ -20,7 +20,7 @@ class long_grippers_server():
self.rate = rospy.Rate(20)
self.result = simulation_control.msg.long_grippersResult()
self.server = actionlib.SimpleActionServer('long_grippers',
- simulation_control.msg.long_grippersAction,
+ autonomous_offboard.msg.long_grippersAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
diff --git a/src/scripts/mavros_state.pyc b/src/scripts/mavros_state.pyc
index ac3d43131589bc9aae5c22ee0a9ef3ddab57e73f..8044321eb231bc91837f7536bc9b1a2e9448d2ce 100644
GIT binary patch
delta 36
ocmZ3bvPy-W`7<w9^^=<$+0XECF*u|%fPu(leKxJl?EK2i0M2p=ApigX
delta 36
ocmZ3bvPy-W`7<xqr>rX*+0XECeXvhw00WW9`fOU8+4+^30pHvT2mk;8
diff --git a/src/scripts/position_control.py b/src/scripts/position_control.py
index 64b7432..dea6744 100755
--- a/src/scripts/position_control.py
+++ b/src/scripts/position_control.py
@@ -7,6 +7,7 @@ from VelocityController import VelocityController
from sensor_msgs.msg import Imu
import numpy as np
+
class position_control():
def __init__(self):
print 'Initialising position control'
@@ -17,7 +18,7 @@ class position_control():
self.actual_height = Float32
self.actual_height = 0.0
- #----------Subscribers----------#
+ # ----------Subscribers----------#
rospy.Subscriber('/position_control/set_mode', String, self.set_mode_callback)
rospy.Subscriber('/position_control/set_position', PoseStamped, self.set_pose_callback)
rospy.Subscriber('/position_control/set_velocity', PoseStamped, self.set_velocity_callback)
@@ -36,7 +37,6 @@ class position_control():
rospy.Subscriber('/mavros/imu/data', Imu, self._local_imu_callback)
rospy.Subscriber('Laser_LidarLite', Float32, self._local_lidar_callback)
-
# pos
self._pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
self._pose_msg = PoseStamped()
@@ -52,33 +52,35 @@ 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._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', PoseStamped, queue_size=10)
+ self.real_local_pose = rospy.Publisher('/position_control/local_pose', Float32, 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.current_publisher = self._pose_pub
self.current_message = self._pose_msg
- #self._pose_msg.pose.position.z = 3
+ # self._pose_msg.pose.position.z = 3
+ self._pose_msg = self.local_pose
+ self._pose_msg.pose.position.x = 0
+ self._pose_msg.pose.position.y = 0
+ self._pose_msg.pose.position.z = 3
self.set_pose(self._pose_msg)
self.des_vel = TwistStamped()
self.current_mode = String()
self.current_mode.data = 'posctr'
self.vController = VelocityController()
- self.vController.set_x_pid(2.8, 0.913921, 0.0, 1) # 0.15
- self.vController.set_y_pid(2.8, 0.913921, 0.0, 1)#2.1, 0.713921, 0.350178
- self.vController.set_z_pid(1.3, 2.4893, 0.102084, 1) # 0.15
- #self.vController.set_yaw_pid(3.6,1.33333,0.1875,1)
+ 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_yaw_pid(3.6,1.33333,0.1875,1)
self.vController.set_yaw_pid(1, 1.33333, 0.1875, 1)
-
print 'Init done'
while not rospy.is_shutdown():
if self.current_mode.data == 'posctr':
@@ -95,7 +97,7 @@ class position_control():
else:
print "No such position mode"
- self.real_local_pose.publish(self.local_pose)
+ self.real_local_pose.publish(self.actual_height)
self.check_distance()
self.get_angles()
rate.sleep()
@@ -109,27 +111,27 @@ class position_control():
self._vel_msg.twist.linear.z = vel.twist.linear.z
def set_pose(self, pose):
- self._pose_msg.pose.position.x = pose.pose.position.x
- self._pose_msg.pose.position.y = pose.pose.position.y
- self._pose_msg.pose.position.z = pose.pose.position.z
+ self._pose_msg.pose.position.x = self.local_pose.pose.position.x + pose.pose.position.x
+ self._pose_msg.pose.position.y = self.local_pose.pose.position.y + pose.pose.position.y
+ self._pose_msg.pose.position.z = pose.pose.position.z - (self.actual_height - self.local_pose.pose.position.z)
- self._pose_msg.pose.orientation.x = pose.pose.orientation.x
- self._pose_msg.pose.orientation.y = pose.pose.orientation.y
- self._pose_msg.pose.orientation.z = pose.pose.orientation.z
- self._pose_msg.pose.orientation.w = pose.pose.orientation.w
+ #self._pose_msg.pose.orientation.x = self._orient_msg.pose.orientation.x
+ #self._pose_msg.pose.orientation.y = self._orient_msg.pose.orientation.y
+ #self._pose_msg.pose.orientation.z = self._orient_msg.pose.orientation.z
+ #self._pose_msg.pose.orientation.w = self._orient_msg.pose.orientation.w
def set_vel_pose(self, vel_pose):
- #print(vel_pose.pose)
+ # 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.pose)
+ # 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
self._vel_pose_msg.pose.orientation.z = vel_pose.pose.orientation.z
self._vel_pose_msg.pose.orientation.w = vel_pose.pose.orientation.w
- def set_velpose_pose(self,vel_pose):
+ def set_velpose_pose(self, vel_pose):
self._velpose_msg.pose.position.x = vel_pose.pose.position.x
self._velpose_msg.pose.position.y = vel_pose.pose.position.y
self._velpose_msg.pose.position.z = vel_pose.pose.position.z
@@ -139,14 +141,9 @@ class position_control():
self._velpose_msg.pose.orientation.z = vel_pose.pose.orientation.z
self._velpose_msg.pose.orientation.w = vel_pose.pose.orientation.w
-
-
def _local_pose_callback(self, data):
self.local_pose = data
- ###Comment out below to get rid of laser height###
- if self.actual_height > 0.0:
- self.local_pose.pose.position.z = self.actual_height
-
+
def _local_velocity_callback(self, data):
self.local_velocity = data
@@ -154,52 +151,46 @@ class position_control():
def _local_imu_callback(self, data):
self.local_imu = data
- def _local_lidar_callback(self,data):
- self.lidar_height = (data.data/100)
+ def _local_lidar_callback(self, data):
+ self.lidar_height = (data.data / 100)
X = self.local_imu.orientation.x
Y = self.local_imu.orientation.y
Z = self.local_imu.orientation.z
W = self.local_imu.orientation.w
- #pitch = math.atan2(2 * OriY * OriW + 2 * OriX * OriZ, 1 - 2 * OriY * OriY - 2 * OriZ * OriZ)
- #roll = math.atan2(2 * OriX * OriW + 2 * OriY * OriZ, 1 - 2 * OriX * OriX - 2 * OriY * OriZ)
+
- 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 = math.atan2(2 * (Y * Z + W * X), W * W - X * X - Y * Y + Z * Z)
+ pitch = math.asin(-2 * (X * Z - W * Y))
- Comp = -math.sin(pitch)*0.22 # 0.22 = 22cm from rotation centrum
+ Comp = -math.sin(pitch) * 0.22 # 0.22 = 22cm from rotation centrum
+
-
- #print self.actual_height
-
- 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
def set_pose_callback(self, data):
self.set_pose(data)
def set_velocity_callback(self, data):
- self.set_vel_pose(data)
+ self.set_vel_pose(data)
- def set_velpose_callback(self,data):
+ def set_velpose_callback(self, data):
self.set_velpose_pose(data)
- def set_x_pid(self,data):
+ def set_x_pid(self, data):
self.vController.set_x_pid(data.x, data.y, data.z)
- def set_y_pid(self,data):
+ def set_y_pid(self, data):
self.vController.set_y_pid(data.x, data.y, data.z)
- def set_z_pid(self,data):
+ def set_z_pid(self, data):
self.vController.set_z_pid(data.x, data.y, data.z)
- def set_yaw_pid(self,data):
+ def set_yaw_pid(self, data):
self.vController.set_yaw_pid(data.x, data.y, data.z)
-
-
def get_angles(self):
X = self.local_imu.orientation.x
@@ -207,15 +198,12 @@ class position_control():
Z = self.local_imu.orientation.z
W = self.local_imu.orientation.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))
+ 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
+ self.pitchangle.publish(math.degrees(pitch)) # Pitch in degrees
self.rollangle.publish(math.degrees(roll)) # Roll in degrees
def check_distance(self):
@@ -228,12 +216,12 @@ class position_control():
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))
+ # 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)
boolvel = self.hover_velocity()
self.dist.publish(booldist and boolvel)
- def is_at_position(self,p_current, p_desired, offset):
+ def is_at_position(self, p_current, p_desired, offset):
des_pos = np.array((p_desired.x,
p_desired.y,
p_desired.z))
@@ -246,6 +234,7 @@ class position_control():
def hover_velocity(self):
return self.local_velocity.twist.linear.x < 0.2 and self.local_velocity.twist.linear.y < 0.2 and self.local_velocity.twist.linear.z < 0.2
+
if __name__ == '__main__':
try:
diff --git a/src/scripts/short_grippers_server.py b/src/scripts/short_grippers_server.py
index e7bf2df..92200df 100755
--- a/src/scripts/short_grippers_server.py
+++ b/src/scripts/short_grippers_server.py
@@ -3,7 +3,7 @@
import roslib
import rospy
import actionlib
-import simulation_control.msg
+import autonomous_offboard.msg
import time
from std_msgs.msg import Float32
@@ -20,7 +20,7 @@ class short_grippers_server():
self.rate = rospy.Rate(20)
self.result = simulation_control.msg.short_grippersResult()
self.server = actionlib.SimpleActionServer('short_grippers',
- simulation_control.msg.short_grippersAction,
+ autonomous_offboard.msg.short_grippersAction,
execute_cb=self.execute_cb,
auto_start=False)
self.server.start()
--
GitLab