Skip to content

Commit

Permalink
Merge pull request #2 from TareqAlqutami/master
Browse files Browse the repository at this point in the history 
using dedicated package for trajectory generation
  • Loading branch information
@TareqAlqutami
TareqAlqutami authored Oct 22, 2024
2 parents f15c957 + 21faaf4 commit 81fef96
Show file tree
Hide file tree
Showing 10 changed files with 625 additions and 104 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
build/
install/
ros2_code/src/px4_msgs
ros2_code/src/traj_gen
matlab/results
log/
__pycache__/
Expand Down
23 changes: 14 additions & 9 deletions media/ros2_workspace_setup.md
View file
Original file line number Diff line number Diff line change
@@ -1,14 +1,19 @@
# ROS2 workspace setup

1. download PX4 msgs dependency and checkout the right version. For px4 v1.15, main branch of px4_msgs throws serialization errors and there is no branch in px4_msgs for px4 v1.15. We checkout a specific commit instead.
```bash
cd ros2_code
git clone https://github.com/PX4/px4_msgs.git src/px4_msgs
cd src/px4_msgs
git checkout bd9dc0fae0162960a31af8f232a9e9f85522ae73
cd ../..
```

1. download dependencies (px4_msgs and traj_gen):
- **px4_msgs** : checkout the right version. For px4 v1.15, main branch of px4_msgs throws serialization errors and there is no branch in px4_msgs for px4 v1.15. We checkout a specific commit instead.
```bash
cd ros2_code
git clone https://github.com/PX4/px4_msgs.git src/px4_msgs
cd src/px4_msgs
git checkout bd9dc0fae0162960a31af8f232a9e9f85522ae73
cd ../..
```
- **traj_gen** (optional): used to generate desired trajectory to be executed by the UAV
```bash
cd ros2_code
git clone https://github.com/TareqAlqutami/traj_gen.git src/traj_gen
```
2. source ROS2

You can use a script to source ros2. We already have one script to source foxy:
Expand Down
2 changes: 1 addition & 1 deletion ros2_code/src/px4_offboard_ros2/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ ros2 launch files description.

7. Switch to offboard mode in QgroundControl or using rqt_reconfigure by ticking `engage_px4_offboard_mode` after running ```bash ros2 run rqt_reconfigure rqt_reconfigure```.

8. Command target pose from rviz marker or use command signal generator
8. Command target pose from rviz marker, use command signal generator, or a trajectory generator such as [this](https://github.com/TareqAlqutami/traj_gen)
```bash
ros2 run px4_offboard_ros2 cmd_sig_generator
```
Expand Down
247 changes: 247 additions & 0 deletions ros2_code/src/px4_offboard_ros2/plotjuggler_layouts/traj_gen_track.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,247 @@
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<Tab tab_name="x" containers="1">
<Container>
<DockSplitter count="2" sizes="0.501149;0.498851" orientation="-">
<DockSplitter count="2" sizes="0.5;0.5" orientation="|">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="8.181660" right="319.720255" bottom="-8.181699" left="73.392382"/>
<limitY/>
<curve color="#1f77b4" name="/target_pose/pose/position/x"/>
<curve color="#ff7f0e" name="/position/vector/x"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="42.188811" right="123.373834" bottom="-42.192017" left="73.392883"/>
<limitY/>
<curve color="#1ac938" name="/target_accel/accel/linear/x"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="2" sizes="0.5;0.5" orientation="|">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="11.969889" right="123.373991" bottom="-19.144334" left="73.393017"/>
<limitY/>
<curve color="#d62728" name="/target_twist/twist/linear/x"/>
</plot>
</DockArea>
<DockSplitter count="2" sizes="0.501155;0.498845" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="110.868157" right="123.373938" bottom="-112.512337" left="73.392959"/>
<limitY/>
<curve color="#ff7f0e" name="/target_jerk/vector/x"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="790.851699" right="123.373964" bottom="-799.173332" left="73.392988"/>
<limitY/>
<curve color="#f14cc1" name="/target_snap/vector/x"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab tab_name="y" containers="1">
<Container>
<DockSplitter count="1" sizes="1" orientation="-">
<DockSplitter count="2" sizes="0.5;0.5" orientation="|">
<DockSplitter count="2" sizes="0.5;0.5" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="2.374701" right="319.720255" bottom="-0.057920" left="73.392382"/>
<limitY/>
<curve color="#bcbd22" name="/target_pose/pose/position/y"/>
<curve color="#f14cc1" name="/position/vector/y"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373991" bottom="-0.100000" left="73.393017"/>
<limitY/>
<curve color="#d62728" name="/target_twist/twist/linear/y"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="3" sizes="0.333717;0.332566;0.333717" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373834" bottom="-0.100000" left="73.392883"/>
<limitY/>
<curve color="#9467bd" name="/target_accel/accel/linear/y"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373938" bottom="-0.100000" left="73.392959"/>
<limitY/>
<curve color="#17becf" name="/target_jerk/vector/y"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373964" bottom="-0.100000" left="73.392988"/>
<limitY/>
<curve color="#1f77b4" name="/target_snap/vector/y"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab tab_name="z" containers="1">
<Container>
<DockSplitter count="1" sizes="1" orientation="-">
<DockSplitter count="2" sizes="0.5;0.5" orientation="|">
<DockSplitter count="2" sizes="0.5;0.5" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="12.249997" right="319.720255" bottom="1.750000" left="73.392382"/>
<limitY/>
<curve color="#f14cc1" name="/target_pose/pose/position/z"/>
<curve color="#9467bd" name="/position/vector/z"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="9.772239" right="123.373991" bottom="-9.771770" left="73.393017"/>
<limitY/>
<curve color="#17becf" name="/target_twist/twist/linear/z"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="3" sizes="0.333717;0.332566;0.333717" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="15.503874" right="123.373834" bottom="-14.967725" left="73.392883"/>
<limitY/>
<curve color="#1ac938" name="/target_accel/accel/linear/z"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="43.079867" right="123.373938" bottom="-43.076478" left="73.392959"/>
<limitY/>
<curve color="#ff7f0e" name="/target_jerk/vector/z"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="387.179514" right="123.373964" bottom="-179.091462" left="73.392988"/>
<limitY/>
<curve color="#9467bd" name="/target_snap/vector/z"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab tab_name="rpy" containers="1">
<Container>
<DockSplitter count="3" sizes="0.333717;0.332566;0.333717" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373906" bottom="-0.100000" left="73.392307"/>
<limitY/>
<curve color="#1ac938" name="/target_att_rpy/vector/x"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373906" bottom="-0.100000" left="73.392307"/>
<limitY/>
<curve color="#d62728" name="/target_att_rpy/vector/y"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="0.100000" right="123.373906" bottom="-0.100000" left="73.392307"/>
<limitY/>
<curve color="#1f77b4" name="/target_att_rpy/vector/z"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<Tab tab_name="rpy_actual" containers="1">
<Container>
<DockSplitter count="3" sizes="0.333717;0.332566;0.333717" orientation="-">
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="178.654686" right="319.720020" bottom="178.224181" left="269.739522"/>
<limitY/>
<curve color="#1f77b4" name="/att_rpy/vector/x"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="1.195260" right="319.720020" bottom="0.850805" left="269.739522"/>
<limitY/>
<curve color="#d62728" name="/att_rpy/vector/y"/>
</plot>
</DockArea>
<DockArea name="...">
<plot flip_y="false" style="Lines" mode="TimeSeries" flip_x="false">
<range top="-83.624075" right="319.720020" bottom="-84.531571" left="269.739522"/>
<limitY/>
<curve color="#1ac938" name="/att_rpy/vector/z"/>
</plot>
</DockArea>
</DockSplitter>
</Container>
</Tab>
<currentTabIndex index="4"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad CSV">
<default delimiter="0" time_axis=""/>
</plugin>
<plugin ID="DataLoad MCAP"/>
<plugin ID="DataLoad ROS2 bags">
<use_header_stamp value="true"/>
<discard_large_arrays value="false"/>
<max_array_size value="9999"/>
<boolean_strings_to_number value="true"/>
<remove_suffix_from_strings value="true"/>
</plugin>
<plugin ID="DataLoad ULog"/>
<plugin ID="ROS2 Topic Subscriber">
<use_header_stamp value="false"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
<boolean_strings_to_number value="true"/>
<remove_suffix_from_strings value="true"/>
</plugin>
<plugin ID="UDP Server"/>
<plugin ID="WebSocket Server"/>
<plugin ID="ZMQ Subscriber"/>
<plugin ID="Fast Fourier Transform"/>
<plugin ID="Quaternion to RPY"/>
<plugin ID="Reactive Script Editor">
<library code="--[[ Helper function to create a series from arrays&#xa;&#xa; new_series: a series previously created with ScatterXY.new(name)&#xa; prefix: prefix of the timeseries, before the index of the array&#xa; suffix_X: suffix to complete the name of the series containing the X value. If [nil], use the index of the array.&#xa; suffix_Y: suffix to complete the name of the series containing the Y value&#xa; timestamp: usually the tracker_time variable&#xa; &#xa; Example:&#xa; &#xa; Assuming we have multiple series in the form:&#xa; &#xa; /trajectory/node.{X}/position/x&#xa; /trajectory/node.{X}/position/y&#xa; &#xa; where {N} is the index of the array (integer). We can create a reactive series from the array with:&#xa; &#xa; new_series = ScatterXY.new(&quot;my_trajectory&quot;) &#xa; CreateSeriesFromArray( new_series, &quot;/trajectory/node&quot;, &quot;position/x&quot;, &quot;position/y&quot;, tracker_time );&#xa;--]]&#xa;&#xa;function CreateSeriesFromArray( new_series, prefix, suffix_X, suffix_Y, timestamp )&#xa; &#xa; --- clear previous values&#xa; new_series:clear()&#xa; &#xa; --- Append points to new_series&#xa; index = 0&#xa; while(true) do&#xa;&#xa; x = index;&#xa; -- if not nil, get the X coordinate from a series&#xa; if suffix_X ~= nil then &#xa; series_x = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_X) )&#xa; if series_x == nil then break end&#xa; x = series_x:atTime(timestamp)&#x9; &#xa; end&#xa; &#xa; series_y = TimeseriesView.find( string.format( &quot;%s.%d/%s&quot;, prefix, index, suffix_Y) )&#xa; if series_y == nil then break end &#xa; y = series_y:atTime(timestamp)&#xa; &#xa; new_series:push_back(x,y)&#xa; index = index+1&#xa; end&#xa;end&#xa;&#xa;--[[ Similar to the built-in function GetSeriesNames(), but select only the names with a give prefix. --]]&#xa;&#xa;function GetSeriesNamesByPrefix(prefix)&#xa; -- GetSeriesNames(9 is a built-in function&#xa; all_names = GetSeriesNames()&#xa; filtered_names = {}&#xa; for i, name in ipairs(all_names) do&#xa; -- check the prefix&#xa; if name:find(prefix, 1, #prefix) then&#xa; table.insert(filtered_names, name);&#xa; end&#xa; end&#xa; return filtered_names&#xa;end&#xa;&#xa;--[[ Modify an existing series, applying offsets to all their X and Y values&#xa;&#xa; series: an existing timeseries, obtained with TimeseriesView.find(name)&#xa; delta_x: offset to apply to each x value&#xa; delta_y: offset to apply to each y value &#xa; &#xa;--]]&#xa;&#xa;function ApplyOffsetInPlace(series, delta_x, delta_y)&#xa; -- use C++ indeces, not Lua indeces&#xa; for index=0, series:size()-1 do&#xa; x,y = series:at(index)&#xa; series:set(index, x + delta_x, y + delta_y)&#xa; end&#xa;end&#xa;"/>
<scripts/>
</plugin>
<plugin ID="CSV Exporter"/>
<plugin ID="ROS2 Topic Re-Publisher"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS2 Topic Subscriber"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets/>
<!-- - - - - - - - - - - - - - - -->
</root>

89 changes: 89 additions & 0 deletions ros2_code/src/px4_offboard_ros2/px4_offboard_ros2/common.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,3 +116,92 @@ def thrust_newton_to_px4(f, r, p):
def thrust_newton_to_px4_real(f):
pwm = (f - 286.8 + 15.48*voltage)/(-0.2563)
return (pwm - 1000.0)/1000.0



class TrapezoidalProfile:
"""
Trajectory generation with trapezoidal velocity profile
"""
def __init__(self,n:int, max_acc:float) -> None:
"""Implements a trapezoidal velocity profile and ensure bounded accelerations.
Args:
n (int): number of dimensions. e.g 3 for cartesian x,y,z motion
max_acc (float): maximum acceleration allowed in all dimensions
"""
self.n = n
self.max_acc = max_acc
self.dt = 0.0
self.vel_sp_trap = np.zeros(self.n)
self.prev_acc = np.zeros(self.n)
self.prev_vel = np.zeros(self.n)
self.vel = np.zeros(self.n)
self.pos = np.zeros(self.n)
self.first_call = True

def reset(self) -> None:
"""Reset internal state for instance due to change in a parameter.
"""
self.dt = 0.0
self.vel_sp_trap = np.zeros(self.n)
self.prev_acc = np.zeros(self.n)
self.prev_vel = np.zeros(self.n)
self.vel = np.zeros(self.n)
self.pos = np.zeros(self.n)
self.first_call = True

def set_max_acc(self,max_acc:float):
"""Change max acceleration value
Args:
max_acc (float): desired max acceleration
"""
self.max_acc = max_acc
self.reset()

def update_vel_sp(self, dt:float, vel_sp:np.array, vel:np.array) -> np.array:
"""Update the velocity set point given raw set point and current velocity
and time since last update
Args:
dt (float): time since last update in seconds
vel_sp (np.array): raw velocity set point
vel (np.array): current velocity
Returns:
np.array: desired velocity set point to achieve trapezoidal profile
"""
if self.first_call:
self.vel_sp_trap = vel # start from current state
self.first_call = False

# calculate acc
acc = (vel_sp - self.vel_sp_trap)/dt
# limit to max acceleration
idx = np.abs(acc)>self.max_acc
acc[idx] = np.sign(acc[idx])*self.max_acc

# calculate velocity set-point satisfying trapezoidal profile
self.vel_sp_trap = self.vel_sp_trap + acc*dt

# estimate position
self.pos += self.vel_sp_trap*dt

return self.vel_sp_trap

def update_pos_sp(self, dt:float, pos_sp:np.array, pos:np.array, vel:np.array) -> np.array:
"""Update the position set point given raw set point and current position and velocity
and time since last update.
Args:
dt (float): time since last update in seconds
pos_sp (np.array): raw position set point
pos (np.array): current position
vel (np.array): current velocity
Returns:
np.array: desired position set point to follow a trapezoidal velocity profile
"""
# TODO
raise NotImplementedError("To be implemented")
2 changes: 1 addition & 1 deletion ros2_code/src/px4_offboard_ros2/px4_offboard_ros2/fa_vel_control_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@
from px4_msgs.msg import VehicleStatus, VehicleAttitude, VehicleLocalPosition, HoverThrustEstimate

from px4_offboard_ros2.PIDn import PIDn
from px4_offboard_ros2.traj_generator import TrapezoidalProfile
from px4_offboard_ros2.common import TrapezoidalProfile
from px4_offboard_ros2.px4_transforms import px4_to_ros_local_frame, px4_to_ros_orientation, ros_to_px4_orientation, ros_to_px4_body_vector

class OffboardVelControl(Node):
Expand Down
Loading

0 comments on commit 81fef96

Please sign in to comment.