Skip to content

added pid controller, added plot juggler to gcs image, refactored how… #279

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

Merged
jfkeller merged 1 commit into from
May 8, 2025
Merged

added pid controller, added plot juggler to gcs image, refactored how… #279

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
2 changes: 1 addition & 1 deletion .env
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
# This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml
PROJECT_NAME="airstack"
# auto-generated from git commit hash
DOCKER_IMAGE_TAG="7c21230"
DOCKER_IMAGE_TAG="f3f93a5"
# can replace with your docker hub username
PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/AFCA/fire_academy_faro_with_sky.scene.usd"
Expand Down
53 changes: 53 additions & 0 deletions common/ros_packages/airstack_common/include/airstack_common/ros2_helper.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,6 +84,59 @@ namespace airstack {
return get_param(node.get(), name, default_value, &set);
}


struct ParamInfo{
void* variable;
rclcpp::ParameterType type;
};
std::unordered_map<std::string, ParamInfo> dynamic_params;
rclcpp::Node* temp_node = NULL;
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle;

rcl_interfaces::msg::SetParametersResult on_param_change(const std::vector<rclcpp::Parameter> & params){
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;

if(temp_node != NULL){
for (const auto & param : params) {
RCLCPP_INFO_STREAM(temp_node->get_logger(), param.get_name());

if(dynamic_params.count(param.get_name()) > 0){
ParamInfo pi = dynamic_params[param.get_name()];

if(pi.type == rclcpp::ParameterType::PARAMETER_INTEGER)
*((int*)pi.variable) = param.as_int();
else if(pi.type == rclcpp::ParameterType::PARAMETER_DOUBLE)
*((double*)pi.variable) = param.as_double();
else if(pi.type == rclcpp::ParameterType::PARAMETER_STRING)
*((std::string*)pi.variable) = param.as_string();
else if(pi.type == rclcpp::ParameterType::PARAMETER_BOOL)
*((bool*)pi.variable) = param.as_bool();
}
}
}



return result;
}

template <typename T>
inline void dynamic_param(rclcpp::Node* node, std::string name, T default_value, T* variable){
temp_node = node;
//rclcpp::ParameterValue pv = node->declare_parameter(name, rclcpp::ParameterValue(default_value));
//*variable = default_value;
*variable = get_param(node, name, default_value);

ParamInfo pi;
pi.variable = (void*)variable;
pi.type = node->get_parameter(name).get_type();//pv.get_type();
dynamic_params[name] = pi;

if(!param_callback_handle)
param_callback_handle = node->add_on_set_parameters_callback(&on_param_change);
}

// services

template <typename T>
Expand Down
1 change: 0 additions & 1 deletion common/ros_packages/straps_msgs
Open in desktop
Submodule straps_msgs deleted from 290987
3 changes: 2 additions & 1 deletion ground_control_station/docker/Dockerfile.gcs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,12 @@ RUN apt-get update && apt-get install -y \
gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-tools \
gstreamer1.0-x gstreamer1.0-alsa openssh-server \
xterm gnome-terminal libcanberra-gtk-module libcanberra-gtk3-module dbus-x11 \
ros-humble-rosbag2-storage-mcap ros-humble-plotjuggler ros-humble-plotjuggler-ros \
&& rm -rf /var/lib/apt/lists/*

# Install Python dependencies
RUN pip3 install empy future lxml matplotlib numpy pkgconfig psutil pygments \
wheel pymavlink pyyaml requests setuptools six toml scipy pytak paho-mqtt sphinx
wheel pymavlink pyyaml requests setuptools six toml scipy pytak paho-mqtt sphinx utm

# Configure SSH
RUN mkdir /var/run/sshd && echo 'root:airstack' | chpasswd && \
Expand Down
205 changes: 205 additions & 0 deletions ground_control_station/ros_ws/src/gcs_bringup/config/control.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,205 @@
<?xml version='1.0' encoding='UTF-8'?>
<root>
<tabbed_widget parent="main_window" name="Main Window">
<Tab containers="1" tab_name="x">
<Container>
<DockSplitter count="2" sizes="0.500543;0.499457" orientation="-">
<DockSplitter count="2" sizes="0.500429;0.499571" orientation="|">
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280194" bottom="-0.000716" top="0.029361" left="431.330560"/>
<limitY/>
<curve name="/robot_1/control/x_pid_info/measured" color="#1f77b4"/>
<curve name="/robot_1/control/x_pid_info/target" color="#d62728"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280194" bottom="-0.000716" top="0.029361" left="431.330560"/>
<limitY/>
<curve name="/robot_1/control/x_pid_info/constant" color="#1ac938"/>
<curve name="/robot_1/control/x_pid_info/control" color="#ff7f0e"/>
<curve name="/robot_1/control/x_pid_info/d_component" color="#f14cc1"/>
<curve name="/robot_1/control/x_pid_info/ff_component" color="#9467bd"/>
<curve name="/robot_1/control/x_pid_info/i_component" color="#17becf"/>
<curve name="/robot_1/control/x_pid_info/p_component" color="#bcbd22"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="2" sizes="0.500429;0.499571" orientation="|">
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280194" bottom="0.006951" top="0.036147" left="431.330560"/>
<limitY/>
<curve name="/robot_1/control/vx_pid_info/measured" color="#17becf"/>
<curve name="/robot_1/control/vx_pid_info/target" color="#bcbd22"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280194" bottom="-0.003713" top="0.004622" left="431.330560"/>
<limitY/>
<curve name="/robot_1/control/vx_pid_info/constant" color="#1f77b4"/>
<curve name="/robot_1/control/vx_pid_info/control" color="#d62728"/>
<curve name="/robot_1/control/vx_pid_info/d_component" color="#1ac938"/>
<curve name="/robot_1/control/vx_pid_info/ff_component" color="#ff7f0e"/>
<curve name="/robot_1/control/vx_pid_info/i_component" color="#f14cc1"/>
<curve name="/robot_1/control/vx_pid_info/p_component" color="#9467bd"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="y">
<Container>
<DockSplitter count="1" sizes="1" orientation="-">
<DockSplitter count="2" sizes="0.500429;0.499571" orientation="|">
<DockSplitter count="2" sizes="0.500543;0.499457" orientation="-">
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280217" bottom="-0.000497" top="0.020366" left="431.330484"/>
<limitY/>
<curve name="/robot_1/control/y_pid_info/measured" color="#1ac938"/>
<curve name="/robot_1/control/y_pid_info/target" color="#ff7f0e"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280217" bottom="-0.000553" top="0.027170" left="431.330484"/>
<limitY/>
<curve name="/robot_1/control/vy_pid_info/measured" color="#f14cc1"/>
<curve name="/robot_1/control/vy_pid_info/target" color="#9467bd"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="2" sizes="0.500543;0.499457" orientation="-">
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280217" bottom="-0.000497" top="0.020366" left="431.330484"/>
<limitY/>
<curve name="/robot_1/control/y_pid_info/constant" color="#1f77b4"/>
<curve name="/robot_1/control/y_pid_info/control" color="#d62728"/>
<curve name="/robot_1/control/y_pid_info/d_component" color="#1ac938"/>
<curve name="/robot_1/control/y_pid_info/ff_component" color="#ff7f0e"/>
<curve name="/robot_1/control/y_pid_info/i_component" color="#f14cc1"/>
<curve name="/robot_1/control/y_pid_info/p_component" color="#9467bd"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280217" bottom="-0.004192" top="0.003980" left="431.330484"/>
<limitY/>
<curve name="/robot_1/control/vy_pid_info/constant" color="#17becf"/>
<curve name="/robot_1/control/vy_pid_info/control" color="#bcbd22"/>
<curve name="/robot_1/control/vy_pid_info/d_component" color="#1f77b4"/>
<curve name="/robot_1/control/vy_pid_info/ff_component" color="#d62728"/>
<curve name="/robot_1/control/vy_pid_info/i_component" color="#1ac938"/>
<curve name="/robot_1/control/vy_pid_info/p_component" color="#ff7f0e"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<Tab containers="1" tab_name="z">
<Container>
<DockSplitter count="1" sizes="1" orientation="-">
<DockSplitter count="2" sizes="0.500429;0.499571" orientation="|">
<DockSplitter count="2" sizes="0.500543;0.499457" orientation="-">
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280239" bottom="-0.004588" top="0.019016" left="431.330607"/>
<limitY/>
<curve name="/robot_1/control/z_pid_info/measured" color="#f14cc1"/>
<curve name="/robot_1/control/z_pid_info/target" color="#9467bd"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280239" bottom="-0.005664" top="0.024910" left="431.330607"/>
<limitY/>
<curve name="/robot_1/control/vz_pid_info/measured" color="#1f77b4"/>
<curve name="/robot_1/control/vz_pid_info/target" color="#d62728"/>
</plot>
</DockArea>
</DockSplitter>
<DockSplitter count="2" sizes="0.500543;0.499457" orientation="-">
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280239" bottom="-0.004588" top="0.019016" left="431.330607"/>
<limitY/>
<curve name="/robot_1/control/z_pid_info/constant" color="#f14cc1"/>
<curve name="/robot_1/control/z_pid_info/control" color="#9467bd"/>
<curve name="/robot_1/control/z_pid_info/d_component" color="#17becf"/>
<curve name="/robot_1/control/z_pid_info/ff_component" color="#bcbd22"/>
<curve name="/robot_1/control/z_pid_info/i_component" color="#1f77b4"/>
<curve name="/robot_1/control/z_pid_info/p_component" color="#d62728"/>
</plot>
</DockArea>
<DockArea name="...">
<plot style="Lines" mode="TimeSeries" flip_y="false" flip_x="false">
<range right="436.280239" bottom="-0.015362" top="0.514345" left="431.330607"/>
<limitY/>
<curve name="/robot_1/control/vz_pid_info/constant" color="#17becf"/>
<curve name="/robot_1/control/vz_pid_info/control" color="#bcbd22"/>
<curve name="/robot_1/control/vz_pid_info/d_component" color="#1f77b4"/>
<curve name="/robot_1/control/vz_pid_info/ff_component" color="#d62728"/>
<curve name="/robot_1/control/vz_pid_info/i_component" color="#1ac938"/>
<curve name="/robot_1/control/vz_pid_info/p_component" color="#ff7f0e"/>
</plot>
</DockArea>
</DockSplitter>
</DockSplitter>
</DockSplitter>
</Container>
</Tab>
<currentTabIndex index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad CSV">
<parameters time_axis="" delimiter="0"/>
</plugin>
<plugin ID="DataLoad MCAP"/>
<plugin ID="DataLoad ROS2 bags">
<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"/>
<selected_topics value=""/>
</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"/>
<selected_topics value="/robot_1/control/vx_pid_info;/robot_1/control/vy_pid_info;/robot_1/control/vz_pid_info;/robot_1/control/x_pid_info;/robot_1/control/y_pid_info;/robot_1/control/z_pid_info"/>
</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>

Loading
Loading