From dcdc1ea47771860e8e097c2b060144dbd6d76cfa Mon Sep 17 00:00:00 2001 From: Seysha Date: Wed, 8 Apr 2026 23:53:55 +0000 Subject: [PATCH] Cleaning --- .gitignore | 4 +- requirements.txt | 3 +- src/Cameras/video_streaming/CMakeLists.txt | 4 - .../include/video_streaming/input_node.hpp | 1 - .../include/video_streaming/rtp_node.hpp | 49 ---- ..._client.launch.py => srt_client.launch.py} | 12 +- .../launch/video_streaming.launch.py | 8 +- .../video_streaming/src/input_node.cpp | 7 - src/Cameras/video_streaming/src/rtp_node.cpp | 219 ------------------ .../launch/controller.launch.py | 2 +- src/kindr | 1 + src/pid_grapher/launch/pid_grapher.launch.py | 14 ++ src/pid_grapher/package.xml | 21 ++ src/pid_grapher/pid_grapher/__init__.py | 0 .../pid_grapher/base_pid_grapher.py | 120 ++++++++++ src/pid_grapher/pid_grapher/joint_data.py | 49 ++++ src/pid_grapher/resource/pid_grapher | 0 src/pid_grapher/setup.cfg | 4 + src/pid_grapher/setup.py | 31 +++ src/pid_grapher/test/test_copyright.py | 27 +++ src/pid_grapher/test/test_flake8.py | 25 ++ src/pid_grapher/test/test_pep257.py | 23 ++ 22 files changed, 331 insertions(+), 293 deletions(-) delete mode 100644 src/Cameras/video_streaming/include/video_streaming/rtp_node.hpp rename src/Cameras/video_streaming/launch/{rtp_client.launch.py => srt_client.launch.py} (62%) delete mode 100644 src/Cameras/video_streaming/src/rtp_node.cpp create mode 160000 src/kindr create mode 100644 src/pid_grapher/launch/pid_grapher.launch.py create mode 100644 src/pid_grapher/package.xml create mode 100644 src/pid_grapher/pid_grapher/__init__.py create mode 100644 src/pid_grapher/pid_grapher/base_pid_grapher.py create mode 100644 src/pid_grapher/pid_grapher/joint_data.py create mode 100644 src/pid_grapher/resource/pid_grapher create mode 100644 src/pid_grapher/setup.cfg create mode 100644 src/pid_grapher/setup.py create mode 100644 src/pid_grapher/test/test_copyright.py create mode 100644 src/pid_grapher/test/test_flake8.py create mode 100644 src/pid_grapher/test/test_pep257.py diff --git a/.gitignore b/.gitignore index b965222e..4a58af14 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ install*/ log/ build*/ +src/kindr *.vscode raman/ @@ -15,4 +16,5 @@ camera_setup/images/* # macOS Directory Information .DS_Store -upgrade_pkg.tar.* \ No newline at end of file +upgrade_pkg.tar.* +src/kindr diff --git a/requirements.txt b/requirements.txt index 911e641b..ce286d24 100644 --- a/requirements.txt +++ b/requirements.txt @@ -19,4 +19,5 @@ onnx onnxruntime onnxslim odrive -smbus2 \ No newline at end of file +smbus2 +matplotlib \ No newline at end of file diff --git a/src/Cameras/video_streaming/CMakeLists.txt b/src/Cameras/video_streaming/CMakeLists.txt index 0cc37cf4..35a0974f 100644 --- a/src/Cameras/video_streaming/CMakeLists.txt +++ b/src/Cameras/video_streaming/CMakeLists.txt @@ -28,8 +28,6 @@ add_library(${PROJECT_NAME} SHARED src/webrtc_node.cpp src/srt_node.cpp src/srt_node_client.cpp - src/rtp_node.cpp - src/rtp_client_node.cpp ) include_directories( @@ -62,8 +60,6 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "WebRtcNode" "video_streaming::SrtNode" "video_streaming::SrtClientNode" - "video_streaming::RtpNode" - "video_streaming::RtpClientNode" ) # ---- Install diff --git a/src/Cameras/video_streaming/include/video_streaming/input_node.hpp b/src/Cameras/video_streaming/include/video_streaming/input_node.hpp index f99df7e7..dc9a3408 100644 --- a/src/Cameras/video_streaming/include/video_streaming/input_node.hpp +++ b/src/Cameras/video_streaming/include/video_streaming/input_node.hpp @@ -43,5 +43,4 @@ class InputNode : public BaseVideoNode { private: std::map source_map_; rclcpp::Service::SharedPtr video_service_; - std::shared_ptr current_video_request_; }; diff --git a/src/Cameras/video_streaming/include/video_streaming/rtp_node.hpp b/src/Cameras/video_streaming/include/video_streaming/rtp_node.hpp deleted file mode 100644 index 409a55c9..00000000 --- a/src/Cameras/video_streaming/include/video_streaming/rtp_node.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include "base_video_node.hpp" - -#include -#include -#include -#include -#include -#include - -namespace video_streaming { - -class RtpNode : public BaseVideoNode { -public: - explicit RtpNode(const rclcpp::NodeOptions &options); - ~RtpNode() override; - -protected: - bool create_pipeline() override; - -private: - rcl_interfaces::msg::SetParametersResult - on_parameter_change(const std::vector ¶meters); - - void set_bitrate(int32_t bitrate); - void set_resolution(int width, int height); - void on_iframe_trigger(const std_msgs::msg::Empty::SharedPtr msg); - std::string get_pipeline_description(); - - int target_framerate_; - int bitrate_; - int fec_; - int dest_port_; - int width_; - int height_; - - std::string dest_ip_; - - GstElement *h265_encoder_{nullptr}; - - rclcpp::Subscription::SharedPtr bitrate_sub_; - rclcpp::Subscription::SharedPtr iframe_sub_; - rclcpp::Publisher::SharedPtr restart_pub_; - - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; -}; - -} // namespace video_streaming \ No newline at end of file diff --git a/src/Cameras/video_streaming/launch/rtp_client.launch.py b/src/Cameras/video_streaming/launch/srt_client.launch.py similarity index 62% rename from src/Cameras/video_streaming/launch/rtp_client.launch.py rename to src/Cameras/video_streaming/launch/srt_client.launch.py index 28d36a52..e940d1c5 100644 --- a/src/Cameras/video_streaming/launch/rtp_client.launch.py +++ b/src/Cameras/video_streaming/launch/srt_client.launch.py @@ -5,21 +5,21 @@ def generate_launch_description(): - rtp_client_node = ComposableNode( + srt_client_node = ComposableNode( package="video_streaming", - plugin="video_streaming::RtpClientNode", - name="rtp_client_node", + plugin="video_streaming::SrtClientNode", + name="srt_client_node", namespace="", - parameters=[{"port": 5004, "latency_ms": 40}], + parameters=[{"srt_uri": "srt://192.168.0.55:7001?mode=caller"}], ) # Container_mt container = ComposableNodeContainer( - name="video_client_container", + name="test_client_container", namespace="", package="rclcpp_components", executable="component_container", - composable_node_descriptions=[rtp_client_node], + composable_node_descriptions=[srt_client_node], output="screen", ) diff --git a/src/Cameras/video_streaming/launch/video_streaming.launch.py b/src/Cameras/video_streaming/launch/video_streaming.launch.py index 06ea4a9e..929445be 100644 --- a/src/Cameras/video_streaming/launch/video_streaming.launch.py +++ b/src/Cameras/video_streaming/launch/video_streaming.launch.py @@ -34,10 +34,10 @@ def generate_launch_description(): parameters=[], ) - rtp_node = ComposableNode( + srt_node = ComposableNode( package="video_streaming", - plugin="video_streaming::RtpNode", - name="rtp_node", + plugin="video_streaming::SrtNode", + name="srt_node", namespace="", parameters=[{"dest_ip": "192.168.0.20", "dest_port": 5004}], ) @@ -52,7 +52,7 @@ def generate_launch_description(): input_node, detect_node, # streaming_node, - rtp_node, + srt_node, ], output="screen", ) diff --git a/src/Cameras/video_streaming/src/input_node.cpp b/src/Cameras/video_streaming/src/input_node.cpp index 565fabac..f778e2ed 100644 --- a/src/Cameras/video_streaming/src/input_node.cpp +++ b/src/Cameras/video_streaming/src/input_node.cpp @@ -89,10 +89,6 @@ bool InputNode::create_pipeline() { } pipeline_ = p; - if (current_video_request_) { - video_cb(current_video_request_, - std::make_shared()); - } return true; } @@ -171,9 +167,6 @@ void InputNode::video_cb( RCLCPP_ERROR(this->get_logger(), "Failed to start pipeline"); response->success = false; } - if (response->success) { - current_video_request_ = request; - } } RCLCPP_COMPONENTS_REGISTER_NODE(InputNode) diff --git a/src/Cameras/video_streaming/src/rtp_node.cpp b/src/Cameras/video_streaming/src/rtp_node.cpp deleted file mode 100644 index be7d7c9c..00000000 --- a/src/Cameras/video_streaming/src/rtp_node.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#include "rtp_node.hpp" -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/int32.hpp" -#include -#include -#include -#include -namespace video_streaming { - -RtpNode::RtpNode(const rclcpp::NodeOptions &options) - : BaseVideoNode("rtp_node", options) { - RCLCPP_INFO(this->get_logger(), "Initializing RtpNode..."); - - this->declare_parameter("dest_ip", "127.0.0.1"); - this->declare_parameter("dest_port", 7001); - this->declare_parameter("target_framerate", 30); - this->declare_parameter("bitrate", 2000000); - this->declare_parameter("fec_percent", 20); - this->declare_parameter("default_width", 1280); - this->declare_parameter("default_height", 720); - - target_framerate_ = this->get_parameter("target_framerate").as_int(); - if (target_framerate_ <= 0) { - RCLCPP_ERROR(this->get_logger(), - "Invalid initial target_framerate: %d. Must be > 0.", - target_framerate_); - throw std::runtime_error("Invalid initial target_framerate"); - } - bitrate_ = this->get_parameter("bitrate").as_int(); - fec_ = this->get_parameter("fec_percent").as_int(); - dest_ip_ = this->get_parameter("dest_ip").as_string(); - dest_port_ = this->get_parameter("dest_port").as_int(); - width_ = this->get_parameter("default_width").as_int(); - height_ = this->get_parameter("default_height").as_int(); - h265_encoder_ = nullptr; - - param_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&RtpNode::on_parameter_change, this, std::placeholders::_1)); - - bitrate_sub_ = this->create_subscription( - "~/set_bitrate", 10, [this](const std_msgs::msg::Int32::SharedPtr msg) { - set_bitrate(msg->data); - }); - - iframe_sub_ = this->create_subscription( - "~/trigger_iframe", 10, - std::bind(&RtpNode::on_iframe_trigger, this, std::placeholders::_1)); - restart_pub_ = this->create_publisher( - "/all_video/restart_pipeline", 10); - - if (!start_pipeline()) { - RCLCPP_FATAL(get_logger(), "RtpNode: failed to start pipeline."); - throw std::runtime_error("RtpNode: pipeline start failed"); - } - RCLCPP_INFO(get_logger(), "RtpNode constructed and pipeline started."); -} - -RtpNode::~RtpNode() { BaseVideoNode::safe_gst_unref(h265_encoder_); } - -std::string RtpNode::get_pipeline_description() { - int video_bitrate = bitrate_ / (1 + fec_ / 100.0); - std::stringstream desc; - desc << "interpipesrc format=3 listen-to=detect is-live=true ! " - << "nvvidconv ! videorate ! " - << "capsfilter caps=\"video/x-raw,framerate=" << target_framerate_ - << "/1\" ! nvvidconv ! " - << "capsfilter caps=\"video/x-raw(memory:NVMM),height=" << height_ - << ",width=" << width_ << "\" ! nvv4l2h265enc bitrate=" << video_bitrate - << " name=h265_enc EnableTwopassCBR=true control-rate=1 " - "maxperf-enable=true ! queue ! rtph265pay config-interval=-1 ! " - "rtpulpfecenc percentage=" - << fec_ << " ! udpsink host=" << dest_ip_ << " port=" << dest_port_; - return desc.str(); -} - -bool RtpNode::create_pipeline() { - std::string desc = get_pipeline_description(); - RCLCPP_INFO(this->get_logger(), "Pipeline description: %s", desc.c_str()); - - GError *err = nullptr; - GstElement *p = gst_parse_launch(desc.c_str(), &err); - - if (err || !p) { - RCLCPP_ERROR(get_logger(), "gst_parse_launch failed: %s", - err ? err->message : "unknown"); - if (err) { - g_error_free(err); - } - return false; - } - if (!GST_IS_PIPELINE(p)) { - RCLCPP_ERROR(get_logger(), "Parsed element is not a pipeline."); - gst_object_unref(p); - return false; - } - pipeline_ = p; - // Unref any existing elements before getting new ones - safe_gst_unref(h265_encoder_); - h265_encoder_ = this->get_element("h265_enc"); - - if (!h265_encoder_) { - RCLCPP_ERROR(get_logger(), "Failed to get elements by name"); - return false; - } - - RCLCPP_INFO(this->get_logger(), - "Pipeline parsed and elements retrieved successfully."); - return true; -} - -rcl_interfaces::msg::SetParametersResult -RtpNode::on_parameter_change(const std::vector ¶meters) { - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - bool needs_restart = false; - - for (const auto ¶m : parameters) { - const std::string &name = param.get_name(); - - // Forward error correction - if (name == "fec_percent") { - fec_ = param.as_int(); - RCLCPP_INFO(this->get_logger(), - "Param Update: Forward Error Correction set to %d%%", fec_); - needs_restart = true; - continue; - } - // Framerate - if (name == "target_framerate") { - target_framerate_ = param.as_int(); - RCLCPP_INFO(this->get_logger(), "Param Update: target_framerate = %d", - target_framerate_); - if (target_framerate_ <= 0) { - result.successful = false; - result.reason = "target_framerate must be greater than 0"; - RCLCPP_ERROR(this->get_logger(), - "Invalid target_framerate: %d. Must be > 0.", - target_framerate_); - return result; - } - needs_restart = true; - continue; - } - // Destination IP - if (name == "dest_ip") { - dest_ip_ = param.as_string(); - RCLCPP_INFO(this->get_logger(), "Param Update: dest_ip = %s", - dest_ip_.c_str()); - needs_restart = true; - continue; - } - // Destination Port - if (name == "dest_port") { - dest_port_ = param.as_int(); - RCLCPP_INFO(this->get_logger(), "Param Update: dest_port = %d", - dest_port_); - needs_restart = true; - continue; - } - // Bitrate - if (name == "bitrate") { - int bitrate = param.as_int(); - set_bitrate(bitrate); - RCLCPP_INFO(this->get_logger(), "Param Update: bitrate = %d", bitrate); - RCLCPP_WARN_ONCE(this->get_logger(), - "Bitrate changes should be made via the " - "~/set_bitrate topic for smoother transitions."); - continue; - } - } - if (needs_restart && result.successful) { - restart_pub_->publish(std_msgs::msg::Empty()); - } - return result; -} - -void RtpNode::set_bitrate(const int32_t bitrate) { - bitrate_ = bitrate; - int video_bitrate = bitrate / (1 + fec_ / 100.0); - if (h265_encoder_) { - pause_pipeline(); - g_object_set(h265_encoder_, "bitrate", video_bitrate, NULL); - resume_pipeline(); - RCLCPP_DEBUG(this->get_logger(), "Bitrate set to %d", video_bitrate); - } - // Adjust resolution based on bitrate thresholds - // TODO: These thresholds are arbitrary and may need tuning based on actual - // performance and quality requirements - int bits_per_frame = video_bitrate / target_framerate_; - if (bits_per_frame < 50000) { - set_resolution(640, 360); - } else if (bits_per_frame < 100000) { - set_resolution(854, 480); - } else if (bits_per_frame < 200000) { - set_resolution(1280, 720); - } else { - set_resolution(1920, 1080); - } -} - -void RtpNode::set_resolution(const int width, const int height) { - if (width == width_ && height == height_) { - return; - } - width_ = width; - height_ = height; - restart_pub_->publish(std_msgs::msg::Empty()); -} - -void RtpNode::on_iframe_trigger(const std_msgs::msg::Empty::SharedPtr msg) { - if (h265_encoder_) { - g_signal_emit_by_name(h265_encoder_, "force-IDR"); - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(video_streaming::RtpNode); -} // namespace video_streaming \ No newline at end of file diff --git a/src/Teleop-Control/joystick_control/launch/controller.launch.py b/src/Teleop-Control/joystick_control/launch/controller.launch.py index bd6ec135..d5bbea9f 100644 --- a/src/Teleop-Control/joystick_control/launch/controller.launch.py +++ b/src/Teleop-Control/joystick_control/launch/controller.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): parameters_file = os.path.join(pkg_joystick_control, "pxn.yaml") # Detect IDs dynamically arm_dev = "/dev/input/by-id/usb-LiteStar_PXN-2113_Pro-joystick" - drive_dev = find_ps5() + drive_dev = "/dev/input/by-id/usb-Thrustmaster_T.16000M-joystick" ld = LaunchDescription() diff --git a/src/kindr b/src/kindr new file mode 160000 index 00000000..22a19ddd --- /dev/null +++ b/src/kindr @@ -0,0 +1 @@ +Subproject commit 22a19ddd30ccfeba72bb2c35361e090bca7d16ec diff --git a/src/pid_grapher/launch/pid_grapher.launch.py b/src/pid_grapher/launch/pid_grapher.launch.py new file mode 100644 index 00000000..dec3b589 --- /dev/null +++ b/src/pid_grapher/launch/pid_grapher.launch.py @@ -0,0 +1,14 @@ +import launch +import launch_ros.actions + + +def generate_launch_description(): + return launch.LaunchDescription( + [ + launch_ros.actions.Node( + package="pid_grapher", + executable="base_pid_grapher", + name="base_pid_grapher_node", + ) + ] + ) diff --git a/src/pid_grapher/package.xml b/src/pid_grapher/package.xml new file mode 100644 index 00000000..b4c33911 --- /dev/null +++ b/src/pid_grapher/package.xml @@ -0,0 +1,21 @@ + + + + pid_grapher + 0.0.0 + TODO: Package description + vscode + TODO: License declaration + + python3-matplotlib + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/pid_grapher/pid_grapher/__init__.py b/src/pid_grapher/pid_grapher/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/pid_grapher/pid_grapher/base_pid_grapher.py b/src/pid_grapher/pid_grapher/base_pid_grapher.py new file mode 100644 index 00000000..5926bdec --- /dev/null +++ b/src/pid_grapher/pid_grapher/base_pid_grapher.py @@ -0,0 +1,120 @@ +import math +from datetime import datetime +from pid_grapher.joint_data import JointData +import rclpy +from rclpy.node import Node +import rclpy.time as time +import matplotlib + +matplotlib.use("TkAgg") +import matplotlib.pyplot as plt +from sensor_msgs.msg import Joy +from sensor_msgs.msg import JointState +import os +from std_msgs.msg import Float64MultiArray + +plt.rcParams["toolbar"] = "none" + + +class BasePIDGrapher(Node): + def update_plots(self): + rate = 10 + period = 1.0 / rate + for j in self.Joints.values(): + j.plotGraph() + self.fig.canvas.draw_idle() + plt.pause(0.000000001) + + def __init__(self): + super().__init__("base_pid_grapher") + self.subscription = self.create_subscription( + JointState, + "/joint_states", + self.plotter_callback, + 10, + ) + self.subscription_targets = self.create_subscription( + Float64MultiArray, + "/arm_controller_servo/commands", + self.trajectory_callback, + 10, + ) + + plt.ion() + + joint_names = ["Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6"] + + n_plots = len(joint_names) + cols = 2 + rows = math.ceil(n_plots / cols) + + self.fig, self.axs = plt.subplots(rows, cols, figsize=(12, rows * 3)) + + # self.fig, self.axs = plt.subplots(3, 2, figsize=(12, 8)) # 3 rows × 2 columns + self.fig.subplots_adjust( + left=0.07, right=0.95, top=0.95, bottom=0.07, hspace=0.3, wspace=0.3 + ) + self.axs = self.axs.flatten() # make it easy to index as a list + self.Joints = {} + for name, ax in zip(joint_names, self.axs): + self.Joints[name] = JointData(name, ax) + + plt.show(block=False) + + self.timer = self.create_timer(0.1, self.update_plots) + + self.subscription + + self.beginning = self.get_clock().now() + + self.fig.canvas.mpl_connect("key_press_event", self.on_key) + + def plotter_callback(self, msg): + now = self.get_clock().now() + + delta = now - self.beginning + x = delta.nanoseconds / 1e9 + for i in range(0, len(msg.name)): + if msg.name[i] in self.Joints: + Joint = self.Joints[msg.name[i]] + Joint.time = x + Joint.state = msg.position[i] + + def _show_plot(self): + plt.show(block=True) + + def trajectory_callback(self, msg): + if len(msg.data) > 0: + jointNames = list(self.Joints.keys()) + for i in range(0, len(jointNames)): + jointName = jointNames[i] + self.Joints[jointName].target = msg.data[i] + + def on_key(self, event): + if event.key == "p": + script_dir = os.path.join(os.getcwd(), "pid_grapher") + plots_dir = os.path.join(script_dir, "plots") + os.makedirs(plots_dir, exist_ok=True) + + timestamp = datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + save_path = os.path.join(plots_dir, f"joint_plot_{timestamp}.png") + + plt.savefig(save_path, bbox_inches="tight", dpi=300) + print(f"Plot saved to {save_path}") + elif event.key == "r": + for j in self.Joints.values(): + j.reset() + self.beginning = self.get_clock().now() + print("Reset data") + + +def main(args=None): + rclpy.init(args=args) + node = BasePIDGrapher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/pid_grapher/pid_grapher/joint_data.py b/src/pid_grapher/pid_grapher/joint_data.py new file mode 100644 index 00000000..8953903a --- /dev/null +++ b/src/pid_grapher/pid_grapher/joint_data.py @@ -0,0 +1,49 @@ +class JointData: + def __init__(self, name, ax): + self.name = name + self.time = 0 + self.target = 0 + self.state = 0 + self.times = [] + self.states = [] + self.targetsHistory = [] + self.ax = ax + self.ax.set_xlabel("Time (s)") + self.ax.set_ylabel("State") + self.ax.set_title(name) + self.ax.legend(["State", "Target"]) + self.ax.grid() + self.line1 = self.ax.plot([], [], "-", label="State")[0] + self.line2 = self.ax.plot([], [], "--", label="Target")[0] + + seconds = 30 # Number of seconds to display on the graph + + self.MAX_POINTS = ( + seconds * 10 + ) - 50 # Maximum number of points to display on the graph (gotten from seconds) + + self.ax.legend() + + def plotGraph(self): + self.targetsHistory.append(round(self.target, 2)) + self.times.append(round(self.time, 2)) + self.states.append(round(self.state, 2)) + + if len(self.times) > self.MAX_POINTS: + self.times.pop(0) + self.states.pop(0) + self.targetsHistory.pop(0) + + self.line1.set_xdata(self.times) + self.line2.set_xdata(self.times) + + self.line1.set_ydata(self.states) + self.line2.set_ydata(self.targetsHistory) + + self.ax.relim() + self.ax.autoscale_view() + + def reset(self): + self.times = [] + self.states = [] + self.targetsHistory = [] diff --git a/src/pid_grapher/resource/pid_grapher b/src/pid_grapher/resource/pid_grapher new file mode 100644 index 00000000..e69de29b diff --git a/src/pid_grapher/setup.cfg b/src/pid_grapher/setup.cfg new file mode 100644 index 00000000..c60af230 --- /dev/null +++ b/src/pid_grapher/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pid_grapher +[install] +install_scripts=$base/lib/pid_grapher diff --git a/src/pid_grapher/setup.py b/src/pid_grapher/setup.py new file mode 100644 index 00000000..1da83986 --- /dev/null +++ b/src/pid_grapher/setup.py @@ -0,0 +1,31 @@ +from setuptools import find_packages, setup +import os +from glob import glob + +package_name = "pid_grapher" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ( + os.path.join("share", package_name, "launch"), + glob(os.path.join("launch", "*launch.[pxy][yma]*")), + ), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools", "matplotlib"], + zip_safe=True, + maintainer="vscode", + maintainer_email="PSPuttaguna@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "pid_grapher=pid_grapher.base_pid_grapher:main", + ], + }, +) diff --git a/src/pid_grapher/test/test_copyright.py b/src/pid_grapher/test/test_copyright.py new file mode 100644 index 00000000..ceffe896 --- /dev/null +++ b/src/pid_grapher/test/test_copyright.py @@ -0,0 +1,27 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip( + reason="No copyright header has been placed in the generated source file." +) +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/src/pid_grapher/test/test_flake8.py b/src/pid_grapher/test/test_flake8.py new file mode 100644 index 00000000..ee79f31a --- /dev/null +++ b/src/pid_grapher/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, "Found %d code style errors / warnings:\n" % len( + errors + ) + "\n".join(errors) diff --git a/src/pid_grapher/test/test_pep257.py b/src/pid_grapher/test/test_pep257.py new file mode 100644 index 00000000..a2c3deb8 --- /dev/null +++ b/src/pid_grapher/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings"