Skip to content

Clean-up of main repo 7 #696

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
johanneskbl wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Clean-up of main repo 7 #696

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 6 additions & 6 deletions aerial_robot_control/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@ generate_dynamic_reconfigure_options(
cfg/LQI.cfg
)

### Temporary soulition because of https://github.com/ros/ros_comm/issues/1404
### Temporary solution because of https://github.com/ros/ros_comm/issues/1404
if (CMAKE_SYSTEM_PROCESSOR MATCHES "(aarch64)|(armhf)" AND $ENV{ROS_DISTRO} MATCHES "melodic")
message(WARNING "please check https://github.com/ros/ros_comm/issues/1404")
message(WARNING "Please check https://github.com/ros/ros_comm/issues/1404")
add_definitions (-DARM_MELODIC)
endif()

Expand All @@ -48,13 +48,13 @@ include_directories(
${EIGEN3_INCLUDE_DIRS}
)

### control utils
### Control utils
add_library(control_utils
src/control/utils/care.cpp
)
target_link_libraries(control_utils ${EIGEN3_LIBRARIES})

### flight control plugin
### Flight control plugin
add_library(flight_control_pluginlib
src/control/base/pose_linear_controller.cpp
src/control/fully_actuated_controller.cpp
Expand All @@ -65,11 +65,11 @@ add_library(flight_control_pluginlib
target_link_libraries(flight_control_pluginlib ${catkin_LIBRARIES} control_utils)
add_dependencies(flight_control_pluginlib ${PROJECT_NAME}_gencfg)

### flight navigation
### Flight navigation
add_library (flight_navigation src/flight_navigation.cpp src/util/joy_parser.cpp)
target_link_libraries (flight_navigation ${catkin_LIBRARIES})

### trajectory generate based on dodgelib
### Trajectory generate based on dodgelib
add_library (trajectory_generation
src/trajectory/reference_base.cpp
src/trajectory/trajectory_reference/polynomial.cpp
Expand Down
139 changes: 68 additions & 71 deletions aerial_robot_control/scripts/circle_trajectroy_following_demo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,111 +1,108 @@
#!/usr/bin/env python

import sys
import time
import rospy
import math
import signal

from aerial_robot_msgs.msg import FlightNav, PoseControlPid

class CircTrajFollow():
def __init__(self):
self.period = rospy.get_param("~period", 40.0)
self.radius = rospy.get_param("~radius", 1.0)
self.init_theta = rospy.get_param("~init_theta", 0.0)
self.yaw = rospy.get_param("~yaw", True)
self.loop = rospy.get_param("~loop", False)
def __init__(self):
self.period = rospy.get_param("~period", 40.0)
self.radius = rospy.get_param("~radius", 1.0)
self.init_theta = rospy.get_param("~init_theta", 0.0)
self.yaw = rospy.get_param("~yaw", True)
self.loop = rospy.get_param("~loop", False)

self.nav_pub = rospy.Publisher("uav/nav", FlightNav, queue_size=1)
self.control_sub = rospy.Subscriber("debug/pose/pid", PoseControlPid, self.controlCb)
self.nav_pub = rospy.Publisher("uav/nav", FlightNav, queue_size=1)
self.control_sub = rospy.Subscriber("debug/pose/pid", PoseControlPid, self.controlCb)

self.center_pos_x = None
self.center_pos_y = None
self.center_pos_x = None
self.center_pos_y = None

self.omega = 2 * math.pi / self.period
self.velocity = self.omega * self.radius
self.omega = 2 * math.pi / self.period
self.velocity = self.omega * self.radius

self.nav_rate = rospy.get_param("~nav_rate", 20.0) # hz
self.nav_rate = 1 / self.nav_rate
self.nav_rate = rospy.get_param("~nav_rate", 20.0) # hz
self.nav_rate = 1 / self.nav_rate

self.flight_nav = FlightNav()
self.flight_nav.target = FlightNav.COG
self.flight_nav.pos_xy_nav_mode = FlightNav.POS_VEL_MODE
if self.yaw:
self.flight_nav.yaw_nav_mode = FlightNav.POS_VEL_MODE
self.flight_nav = FlightNav()
self.flight_nav.target = FlightNav.COG
self.flight_nav.pos_xy_nav_mode = FlightNav.POS_VEL_MODE
if self.yaw:
self.flight_nav.yaw_nav_mode = FlightNav.POS_VEL_MODE

signal.signal(signal.SIGINT, self.stopRequest)
signal.signal(signal.SIGINT, self.stopRequest)

time.sleep(0.5)
time.sleep(0.5)

def controlCb(self, msg):
def controlCb(self, msg):

self.initial_target_yaw = msg.yaw.target_p
self.initial_target_yaw = msg.yaw.target_p

self.center_pos_x = msg.x.target_p - math.cos(self.init_theta) * self.radius
self.center_pos_y = msg.y.target_p - math.sin(self.init_theta) * self.radius
self.center_pos_x = msg.x.target_p - math.cos(self.init_theta) * self.radius
self.center_pos_y = msg.y.target_p - math.sin(self.init_theta) * self.radius

rospy.loginfo("the center position is [%f, %f]", self.center_pos_x, self.center_pos_y)
rospy.loginfo("the center position is [%f, %f]", self.center_pos_x, self.center_pos_y)

self.control_sub.unregister()
self.control_sub.unregister()

def stopRequest(self, signal, frame):
rospy.loginfo("stop following")
self.flight_nav.target_vel_x = 0
self.flight_nav.target_vel_y = 0
self.flight_nav.target_omega_z = 0
self.nav_pub.publish(self.flight_nav)
def stopRequest(self, signal, frame):
rospy.loginfo("stop following")
self.flight_nav.target_vel_x = 0
self.flight_nav.target_vel_y = 0
self.flight_nav.target_omega_z = 0
self.nav_pub.publish(self.flight_nav)

sys.exit(0)
sys.exit(0)

def main(self):
def main(self):

cnt = 0
cnt = 0

while not rospy.is_shutdown():
while not rospy.is_shutdown():

if self.center_pos_y is None:
rospy.loginfo_throttle(1.0, "not yet receive the controller message")
time.sleep(self.nav_rate)
continue
if self.center_pos_y is None:
rospy.loginfo_throttle(1.0, "not yet receive the controller message")
time.sleep(self.nav_rate)
continue

theta = self.init_theta + cnt * self.nav_rate * self.omega
self.target_pos_x = self.center_pos_x + math.cos(theta) * self.radius
self.target_pos_y = self.center_pos_y + math.sin(theta) * self.radius
self.flight_nav.target_pos_x = self.target_pos_x
self.flight_nav.target_pos_y = self.target_pos_y
self.flight_nav.target_vel_x = -math.sin(theta) * self.velocity
self.flight_nav.target_vel_y = math.cos(theta) * self.velocity
theta = self.init_theta + cnt * self.nav_rate * self.omega
self.target_pos_x = self.center_pos_x + math.cos(theta) * self.radius
self.target_pos_y = self.center_pos_y + math.sin(theta) * self.radius
self.flight_nav.target_pos_x = self.target_pos_x
self.flight_nav.target_pos_y = self.target_pos_y
self.flight_nav.target_vel_x = -math.sin(theta) * self.velocity
self.flight_nav.target_vel_y = math.cos(theta) * self.velocity

if self.yaw:
self.flight_nav.target_yaw = self.initial_target_yaw + cnt * self.nav_rate * self.omega
self.flight_nav.target_omega_z = self.omega
if self.yaw:
self.flight_nav.target_yaw = self.initial_target_yaw + cnt * self.nav_rate * self.omega
self.flight_nav.target_omega_z = self.omega

self.nav_pub.publish(self.flight_nav)
self.nav_pub.publish(self.flight_nav)

cnt += 1
cnt += 1

if cnt == self.period // self.nav_rate:
if self.loop:
cnt = 0
else:
if cnt == self.period // self.nav_rate:
if self.loop:
cnt = 0
else:

time.sleep(0.1)
self.flight_nav.target_vel_x = 0
self.flight_nav.target_vel_y = 0
self.flight_nav.target_omega_z = 0
self.nav_pub.publish(self.flight_nav)
time.sleep(0.1)
self.flight_nav.target_vel_x = 0
self.flight_nav.target_vel_y = 0
self.flight_nav.target_omega_z = 0
self.nav_pub.publish(self.flight_nav)

break # only one loop
break # only one loop

time.sleep(self.nav_rate)
time.sleep(self.nav_rate)


if __name__ == "__main__":

rospy.init_node("circle_trajectory_follow")

Tracker = CircTrajFollow()
Tracker.main()


rospy.init_node("circle_trajectory_follow")

Tracker = CircTrajFollow()
Tracker.main()
Loading