diff --git a/.gitignore b/.gitignore index abdf307..7a7a0e0 100755 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ -/build -/devel +*/build +*/install +*/log /.vscode /src/CmakeLists.txt *.catkin_workspace \ No newline at end of file diff --git a/README.md b/README.md index 3df15b6..afe9fc9 100755 --- a/README.md +++ b/README.md @@ -1,11 +1,10 @@ # how to use ### build docker environment - `cd Winter-Tutorial/docker` -- `docker build -t dit/tutorial-ws-noetic:latest .` -- `docker compose up` -- `docker start tutorial-ws-ros1` +- `docker compose up -d` ### open work space -- Attach Visual Studio Code -- `cd tutorial_ws` -- `source /opt/ros/noetic/setup.bash` -- `catkin_make` +- `docker exec -it ros2_tutorial bash` +- `cd ~/ros2_ws` +- `colcon build` +### launch +- `ros2 launch turtle_square_control turtle_launch.py` diff --git a/cpp_oop/Makefile b/cpp_oop/Makefile new file mode 100644 index 0000000..fe0fdda --- /dev/null +++ b/cpp_oop/Makefile @@ -0,0 +1,38 @@ +# Makefile for C++ OOP Tutorial +# Demonstrates header files, source files, enum, and extern + +CXX = g++ +CXXFLAGS = -Wall -Wextra -std=c++14 -I./include +TARGET = vehicle_demo + +# Source files +SRCS = src/Vehicle.cpp src/Car.cpp src/Airplane.cpp src/main.cpp + +# Object files +OBJS = $(SRCS:.cpp=.o) + +# Default target +all: $(TARGET) + +# Link object files to create executable +$(TARGET): $(OBJS) + $(CXX) $(CXXFLAGS) -o $(TARGET) $(OBJS) + @echo "Build complete! Run with: ./$(TARGET)" + +# Compile source files to object files +%.o: %.cpp + $(CXX) $(CXXFLAGS) -c $< -o $@ + +# Run the program +run: $(TARGET) + ./$(TARGET) + +# Clean build files +clean: + rm -f $(OBJS) $(TARGET) + @echo "Clean complete!" + +# Rebuild everything +rebuild: clean all + +.PHONY: all run clean rebuild diff --git a/cpp_oop/README.md b/cpp_oop/README.md new file mode 100644 index 0000000..7516351 --- /dev/null +++ b/cpp_oop/README.md @@ -0,0 +1,110 @@ +# C++ OOP Tutorial + +2025 Winter Turtorial Supplementary C++, OOP的範例程式 + +## Topics Covered + +### 1. OOP (Object-Oriented Programming) +- **Encapsulation**: Private members in `Car` and `Airplane` classes +- **Inheritance**: `Car` and `Airplane` inherit from `Vehicle` +- **Polymorphism**: Virtual function `move()` with different implementations + +### 2. Header File (.h) & Source File (.cpp) + +#### Header File (`.h`) +- 放 declarations（宣告）、include、extern +- 別人看 header file 就可以知道有什麼功能 +- 統一放在資料夾`include` + +例如: `Vehicle.h`, `Car.h`, `Airplane.h` + +#### Source File (`.cpp`) +- 實際地把 header file 中的東西 implement 出來 +- 放在資料夾`src` +- `main.cpp`是主程式，因為只有`main()`所以我沒寫`main.h` + +例如: `Vehicle.cpp`, `Car.cpp`, `Airplane.cpp` + + +### 3. enum (Enumeration) + +enum 是一個 user-defined type，代表一組有限的命名整數。 + + +**用途:** +- 劃分不同的類別或狀態 +- 讓程式碼更容易閱讀和維護 +- 範例 1: Color +- 範例 2: VehicleStatus +- 都在`Vehicle.h` + + +### 4. Global Variable & extern + +**重要規則:** +- extern 只要宣告一次 +- 接下來都用 include header file 的方式 +- **初始化只要一次**（多個程式嘗試初始化會造成問題！） + +**本專案範例:** +- `Vehicle.h`: 宣告 `extern int global_vehicle_count;` +- `Vehicle.cpp`: 初始化 `int global_vehicle_count = 0;` +- `main.cpp`: 透過 include `Car.h` (which includes `Vehicle.h`) 使用這個變數 + +## Project Structure + +``` +cpp_oop/ +├── include/ +│ ├── Vehicle.h # base class + enum + extern 宣告(declaration) +│ ├── Car.h # Car class declaration +│ └── Airplane.h # Airplane declaration +├── src/ +│ ├── Vehicle.cpp # Vehicle 實作(implementation) + global 變數初始化 +│ ├── Car.cpp # Car implementation +│ ├── Airplane.cpp # Airplane implementation +│ └── main.cpp # 主程式 +├── Makefile # 編譯腳本 +└── README.md # 本檔案 +``` + +## How to Build and Run + +- 由於有多個檔案需要編譯，我叫AI寫了`Makefile` +- `Makefile`可以包含編譯的指令，給`make`工具使用 +- 學ROS時會有`CMakeLists.txt`，功能比Makefile更多，編譯過程中會自動生成`Makefile` +- 不需要打開`Makefile` +- 想要自己用`g++`之類的編譯請問AI + +### Entering the directory in docker container +```bash +# 以免有人本地端無法編譯C++，這邊的指令是在container裡編譯 +# 編譯管道與ROS無關，只是單純的 make工具 與 g++ compiler + +### Build docker environement(Same as the homepage READE.md) +- `cd docker` +- docker compose up -d +- *Container ros2_tutorial Running* # expected o/p, dont type +- docker exec -it ros2_tutorial bash + +### Enter cpp_oop directory +cd cpp_oop +``` + +### Using Makefile +```bash +# type the command in terminal + +# Build the project +make + +# Build and run +make run + +# Clean build files +make clean + +# Rebuild from scratch +make rebuild +``` + diff --git a/cpp_oop/include/Airplane.h b/cpp_oop/include/Airplane.h new file mode 100644 index 0000000..ff1326f --- /dev/null +++ b/cpp_oop/include/Airplane.h @@ -0,0 +1,23 @@ +// to avoid multiple include (include guard) +#ifndef AIRPLANE_H +#define AIRPLANE_H + +#include "Vehicle.h" + +// Airplane is-a Vehicle, so Airplane inherits from Vehicle just like Car +class Airplane : public Vehicle { +private: + int altitude; + +public: + // Constructor of Airplane + Airplane(int fuel, int max_speed, int altitude, Color color = BLUE); + + // 除了inherited from Vehicle的public getter/setter function，Airplane還有自己的函式 + void climb(int meters); + int getAltitude() const; + + std::string move() const override; // declare with override to enable polymorphism +}; + +#endif diff --git a/cpp_oop/include/Car.h b/cpp_oop/include/Car.h new file mode 100644 index 0000000..5a200e0 --- /dev/null +++ b/cpp_oop/include/Car.h @@ -0,0 +1,24 @@ +// to avoid multiple include (include guard) +#ifndef CAR_H +#define CAR_H + +#include "Vehicle.h" // to use class declaration of Vehicle + + +// Car is-a Vehicle, so Car inherits from Vehicle +class Car : public Vehicle { +private: + int seat_count; // encapsulated + +public: + // Constructor with color parameter + Car(int fuel, int max_speed, int seat_count, Color color = RED); + + // 除了inherited from Vehicle的public getter/setter function，Car還有自己的函式 + void drive(); + int getSeatCount() const; + + std::string move() const override; // declare with override to enable polymorphism +}; + +#endif diff --git a/cpp_oop/include/Vehicle.h b/cpp_oop/include/Vehicle.h new file mode 100644 index 0000000..16c9ac3 --- /dev/null +++ b/cpp_oop/include/Vehicle.h @@ -0,0 +1,60 @@ +// to avoid multiple include (include guard) +#ifndef VEHICLE_H +#define VEHICLE_H + +#include + +// enum: a user-defined type that represents a finite set of named integer +enum Color { + RED, // 預設 RED==0 + GREEN, // GREEN==1 + BLUE, // BLUE==2 + YELLOW, + BLACK, + WHITE +}; + +// 用來表示交通工具的狀態 +enum VehicleStatus { + IDLE = 0, + MOVING = 1, + REFUELING = 2, + MAINTENANCE = 3 +}; + +// 在.h宣告變數extern但不初始化 +extern int global_vehicle_count; + +// 交通工具（抽象概念） +class Vehicle { +protected: + int fuel; // 燃料 + int max_speed; // 最大速度 + Color color; // 顏色 (使用 enum) + VehicleStatus status; // 狀態 (使用 enum) + +public: + Vehicle(int fuel, int max_speed, Color color = WHITE); + virtual ~Vehicle(); // destructor + + void refuel(int amount); + + // getter function (get the values of these variables but not directly accessing them) + int getFuel() const; // const -> this function should not modify the variable in the class + int getMaxSpeed() const; + Color getColor() const; + VehicleStatus getStatus() const; + + // setter function (set the values of specified varaible) + void setStatus(VehicleStatus new_status); + + // demo how to use enum for if-statement + std::string getColorName() const; + + // Polymorphism interface, declared virtual + // this is pure virtual function(so the child of Vehicle must declare own move function) + // pure virtual function does not need function definition here + virtual std::string move() const = 0; +}; + +#endif diff --git a/cpp_oop/src/Airplane.cpp b/cpp_oop/src/Airplane.cpp new file mode 100644 index 0000000..e738aa9 --- /dev/null +++ b/cpp_oop/src/Airplane.cpp @@ -0,0 +1,23 @@ +#include "Airplane.h" + +Airplane::Airplane(int fuel, int max_speed, int altitude, Color color) + : Vehicle(fuel, max_speed, color), altitude(altitude) {} + +void Airplane::climb(int meters) { + if (fuel > 0) { + status = MOVING; // 使用 enum 設定狀態 + altitude += meters; + fuel--; + } + else { + status = IDLE; + } +} + +int Airplane::getAltitude() const { + return altitude; +} + +std::string Airplane::move() const { + return "Airplane flies in the sky"; +} diff --git a/cpp_oop/src/Car.cpp b/cpp_oop/src/Car.cpp new file mode 100644 index 0000000..fb61fd5 --- /dev/null +++ b/cpp_oop/src/Car.cpp @@ -0,0 +1,24 @@ +#include "Car.h" +// no need to include Vehicle.h, since Car.h has included Vehicle.h + +Car::Car(int fuel, int max_speed, int seat_count, Color color) + : Vehicle(fuel, max_speed, color), seat_count(seat_count) {} + // Car's constructor must call the constructor of the parent(Vehicle) + +void Car::drive() { + if (fuel > 0) { + status = MOVING; // 使用 enum 設定狀態 + fuel--; // protected member + } + else { + status = IDLE; + } +} + +int Car::getSeatCount() const { + return seat_count; +} + +std::string Car::move() const { + return "Car drives on the road"; +} diff --git a/cpp_oop/src/Vehicle.cpp b/cpp_oop/src/Vehicle.cpp new file mode 100644 index 0000000..9dba07b --- /dev/null +++ b/cpp_oop/src/Vehicle.cpp @@ -0,0 +1,65 @@ +#include "Vehicle.h" +// Vehicle.cpp hava the function definition +// and variable initialization +// corresponding to Vehicle.h + +// 初始化extern過的variable +int global_vehicle_count = 0; + +Vehicle::Vehicle(int fuel, int max_speed, Color color) + : fuel(fuel), max_speed(max_speed), color(color), status(IDLE) { + global_vehicle_count++; // 每創建一個交通工具，總數+1 +} + +Vehicle::~Vehicle() { + global_vehicle_count--; // 交通工具被刪除時，總數-1 +} + +void Vehicle::refuel(int amount) { + status = REFUELING; + fuel += amount; + status = IDLE; +} + +int Vehicle::getFuel() const { + return fuel; +} + +int Vehicle::getMaxSpeed() const { + return max_speed; +} + +Color Vehicle::getColor() const { + return color; +} + +VehicleStatus Vehicle::getStatus() const { + return status; +} + +void Vehicle::setStatus(VehicleStatus new_status) { + status = new_status; +} + +// ===== 使用 enum 來判斷 ===== +std::string Vehicle::getColorName() const { + if (color == Color::RED) { + return "Red"; + } + else if (color == Color::GREEN) { + return "Green"; + } + else if (color == Color::BLUE) { + return "Blue"; + } + else if (color == Color::YELLOW) { + return "Yellow"; + } + else if (color == Color::BLACK) { + return "Black"; + } + else if (color == Color::WHITE) { + return "White"; + } + return "Unknown"; +} diff --git a/cpp_oop/src/main.cpp b/cpp_oop/src/main.cpp new file mode 100644 index 0000000..bd9bcbb --- /dev/null +++ b/cpp_oop/src/main.cpp @@ -0,0 +1,54 @@ +#include +#include +#include "Car.h" +#include "Airplane.h" + +using namespace std; + +int main() { + cout << "===== C++ OOP Tutorial =====" << endl; + cout << "Topics: Header/Source files, enum, extern, OOP" << endl << endl; + + // to see the initialization of extern global variable + cout << "Global vehicle count: " << global_vehicle_count << endl; + + // enum simple usage + Color my_color = Color::RED; + if (my_color == Color::RED) { + cout << "my Color is RED!" << endl; + } + + // OOP demo + Car* v1 = new Car(100, 180, 5, Color::RED); + Airplane* v2 = new Airplane(300, 900, 1000, Color::BLUE); + // 也可以用smart pointer + // unique_ptr v1 = make_unique(100, 180, 5, Color::RED); + // unique_ptr v2 = make_unique(300, 900, 1000, Color::BLUE); + + cout << v1->move() << endl; + cout << v2->move() << endl; + + cout << "Car color: " << v1->getColorName() << endl; + cout << "Airplane color: " << v2->getColorName() << endl; + + cout << "Global count: " << global_vehicle_count << endl; + + // this method is inherited from Vehicle + cout << "Car Fuel: " << v1->getFuel() << endl; + cout << "Airplane maxSpeed: " << v2->getMaxSpeed() << endl; + + // these methods are additional function + cout << "Car SeatCount: " << v1->getSeatCount() << endl; + cout << "Airplane Altitude: " << v2->getAltitude() << endl; + + + // try on your own to call Car, Airplane's other function + + + // if use smart pointer, no need to delete, + // since system automatically release memory + delete v1; + delete v2; + + return 0; +} diff --git a/docker/Dockerfile b/docker/Dockerfile index 02c02dc..67db515 100755 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,31 +1,46 @@ -FROM osrf/ros:noetic-desktop-full +FROM osrf/ros:humble-desktop + +LABEL shell=bash + +ARG USERNAME=user +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Create the user +# add user with default bash +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ + && apt-get update \ + && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* # Install common tools RUN apt-get update && apt-get install -y \ curl \ git \ - git-extras \ - htop \ tree \ net-tools \ + usbutils \ tmux \ vim \ wget \ python3-pip \ + ros-humble-rqt-tf-tree \ bash-completion -RUN apt-get update && apt-get install -y \ - ros-noetic-dynamixel-sdk \ - ros-noetic-turtlebot3-msgs \ - ros-noetic-turtlebot3 \ - ros-noetic-gazebo-ros-pkgs +RUN echo "source /opt/ros/humble/setup.bash" >> /etc/bash.bashrc \ + && echo "source /home/user/ros_tutorial/ros2_ws/install/setup.bash" >> /etc/bash.bashrc RUN apt-get update && apt-get dist-upgrade -y \ && apt-get autoremove -y \ && apt-get autoclean -y \ && rm -rf /var/lib/apt/lists/* -ENV SHELL /bin/bash +ENV SHELL=/bin/bash ENV TERM=xterm-256color -CMD ["/bin/bash"] \ No newline at end of file +USER $USERNAME + +CMD ["/bin/bash"] diff --git a/docker/compose.yaml b/docker/compose.yaml new file mode 100644 index 0000000..1a2cd49 --- /dev/null +++ b/docker/compose.yaml @@ -0,0 +1,30 @@ +name: ros2 +services: + ros2: + build: + context: . + args: + USERNAME: user + image: dit/ros2_tutorial:latest + container_name: ros2_tutorial + stdin_open: true + tty: true + privileged: true + stop_grace_period: 1s + + network_mode: host + working_dir: /home/user/ros_tutorial + environment: + - DISPLAY=host.docker.internal:0.0 + - XAUTHORITY=/root/.Xauthority + - ROS_WS=/home/user/ros_tutorial/ros2_ws + volumes: + - /etc/timezone:/etc/timezone:ro + - /etc/localtime:/etc/localtime:ro + # Mount X11 server + - /d/.Xauthority:/root/.Xauthority + - /tmp/.X11-unix:/tmp/.X11-unix:rw + # Direct Rendering Infrastructure + - /dev:/dev + - ../:/home/user/ros_tutorial + command: bash diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml deleted file mode 100755 index 882d54f..0000000 --- a/docker/docker-compose.yaml +++ /dev/null @@ -1,34 +0,0 @@ -version: '3' -services: - ros1-tutorial-ws: - build: - context: . - dockerfile: Dockerfile - args: - image: dit/tutorial-ws-noetic:latest - container_name: tutorial-ws-ros1 - stdin_open: true - tty: true - privileged: true - - network_mode: host - working_dir: /home/user/tutorial_ws - environment: - - DISPLAY=host.docker.internal:0.0 - - XAUTHORITY=/root/.Xauthority - - ROS_WS=/home/user/tutorial_ws - volumes: - # Mount local timezone into container. ( Readonly ) - # Reference: https://stackoverflow.com/questions/57607381/how-do-i-change-timezone-in-a-docker-container - - /etc/timezone:/etc/timezone:ro - - /etc/localtime:/etc/localtime:ro - # Mount X11 server - - /d/.Xauthority:/root/.Xauthority - - /tmp/.X11-unix:/tmp/.X11-unix:rw - # Direct Rendering Infrastructure - - /dev:/dev - - ../:/home/user/tutorial_ws - devices: - - /dev/bus/usb - - command: /bin/bash -c "source /opt/ros/noetic/setup.bash && exec bash" diff --git a/ros2_ws/src/cpp_action/CMakeLists.txt b/ros2_ws/src/cpp_action/CMakeLists.txt new file mode 100644 index 0000000..af1766e --- /dev/null +++ b/ros2_ws/src/cpp_action/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_action) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(example_interfaces REQUIRED) +find_package(std_msgs REQUIRED) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_executable(integrated_node + src/counter_client.cpp + src/counter_server.cpp + src/main.cpp +) + +add_executable(integrated_node_1 + src/counter_client.cpp + src/counter_server.cpp + src/main1.cpp +) + +target_include_directories(integrated_node PUBLIC + $ + $ +) + +target_include_directories(integrated_node_1 PUBLIC + $ + $ +) + +ament_target_dependencies(integrated_node + rclcpp + rclcpp_action + example_interfaces + std_msgs +) + +ament_target_dependencies(integrated_node_1 + rclcpp + rclcpp_action + example_interfaces + std_msgs +) + +add_executable(thread_example src/thread_example.cpp) +find_package(Threads REQUIRED) +target_link_libraries(thread_example Threads::Threads) + +add_executable(thread_example_1 src/thread_example_1.cpp) +find_package(Threads REQUIRED) +target_link_libraries(thread_example_1 Threads::Threads) + +install(TARGETS + integrated_node + thread_example + integrated_node_1 + thread_example_1 + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros2_ws/src/cpp_action/README.md b/ros2_ws/src/cpp_action/README.md new file mode 100644 index 0000000..b627d6d --- /dev/null +++ b/ros2_ws/src/cpp_action/README.md @@ -0,0 +1,5 @@ +1. execute an action client ask for server to count to a specific target number +2. the action server feedbask the time spent and return the result as the target number +3. execute another node that count the numbers of prime number under the target number +4. wrap the counter node and prime number finding node in a same multi-threaded executor +5. all these three nodes added into same component \ No newline at end of file diff --git a/ros2_ws/src/cpp_action/include/cpp_action/counter_client.hpp b/ros2_ws/src/cpp_action/include/cpp_action/counter_client.hpp new file mode 100644 index 0000000..7fb1c3d --- /dev/null +++ b/ros2_ws/src/cpp_action/include/cpp_action/counter_client.hpp @@ -0,0 +1,41 @@ +#ifndef COUNTER_SERVER_HPP_ +#define COUNTER_SERVER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "example_interfaces/action/counter.hpp" +#include "std_msgs/msg/int32.hpp" + +class CounterActionClient : public rclcpp::Node +{ +public: + explicit CounterActionClient(const rclcpp::NodeOptions & options) + : Node("example_action_client", options) { + this->action_client_ = rclcpp_action::create_client( + this, "count_to"); + publisher_ = this->create_publisher("target_num", 10); + + this->timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&CounterActionClient::send_goal, this)); + } + + void send_goal(); + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp_action::Client::SharedPtr action_client_; + rclcpp::TimerBase::SharedPtr timer_; + + void goal_response_callback(std::shared_future::SharedPtr> future); + + void feedback_callback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback); + + void result_callback(const rclcpp_action::ClientGoalHandle + ::WrappedResult & result); +}; + +#endif \ No newline at end of file diff --git a/ros2_ws/src/cpp_action/include/cpp_action/counter_server.hpp b/ros2_ws/src/cpp_action/include/cpp_action/counter_server.hpp new file mode 100644 index 0000000..500b260 --- /dev/null +++ b/ros2_ws/src/cpp_action/include/cpp_action/counter_server.hpp @@ -0,0 +1,60 @@ +#ifndef COUNTER_CLIENT_HPP_ +#define COUNTER_CLIENT_HPP_ + +#include +#include +#include "rcl_action/action_server.h" +#include "example_interfaces/action/counter.hpp" +#include "std_msgs/msg/int32.hpp" + +class CounterActionServer : public rclcpp::Node +{ +public: + CounterActionServer(const rclcpp::NodeOptions & options) + : Node("counter_server", options) { + // use Reentrant and create a callback group + callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + this->action_server_ = rclcpp_action::create_server( + this, + "count_to", + std::bind(&CounterActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&CounterActionServer::handle_cancel, this, std::placeholders::_1), + std::bind(&CounterActionServer::handle_accepted, this, std::placeholders::_1), + rcl_action_server_get_default_options(), + callback_group_ + ); + } + +private: + double start_time, current_time; + int current_number; + rclcpp_action::Server::SharedPtr action_server_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + + rclcpp_action::GoalResponse handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel( + const std::shared_ptr> goal_handle); + void handle_accepted(const std::shared_ptr> goal_handle); +}; + +class PrimeFinderSubscriber : public rclcpp::Node +{ +public: + PrimeFinderSubscriber(const rclcpp::NodeOptions & options) : Node("prime_finder", options) { + auto cb_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + auto sub_options = rclcpp::SubscriptionOptions(); + sub_options.callback_group = cb_group; + + subscription_ = this->create_subscription( + "target_num", 10, std::bind(&PrimeFinderSubscriber::calculate_primes, this, std::placeholders::_1)); + } + +private: + rclcpp::Subscription::SharedPtr subscription_; + + void calculate_primes(const std_msgs::msg::Int32::SharedPtr msg); +}; + +#endif \ No newline at end of file diff --git a/ros2_ws/src/cpp_action/package.xml b/ros2_ws/src/cpp_action/package.xml new file mode 100644 index 0000000..9b14137 --- /dev/null +++ b/ros2_ws/src/cpp_action/package.xml @@ -0,0 +1,23 @@ + + + + cpp_action + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + example_interfaces + rclcpp + rclcpp_action + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_ws/src/cpp_action/src/counter_client.cpp b/ros2_ws/src/cpp_action/src/counter_client.cpp new file mode 100644 index 0000000..e301b13 --- /dev/null +++ b/ros2_ws/src/cpp_action/src/counter_client.cpp @@ -0,0 +1,65 @@ +#include "cpp_action/counter_client.hpp" + +void CounterActionClient::send_goal() +{ + this->timer_->cancel(); + + auto message = std_msgs::msg::Int32(); + message.data = 100000; + RCLCPP_INFO(this->get_logger(), "Publishing: %d", message.data); + publisher_->publish(message); + + if (!this->action_client_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = example_interfaces::action::Counter::Goal(); + goal_msg.target_number = 100000; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + // send_goal_options.goal_response_callback = + // std::bind(&CounterActionClient::goal_response_callback, this, std::placeholders::_1); + send_goal_options.feedback_callback = + std::bind(&CounterActionClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + send_goal_options.result_callback = + std::bind(&CounterActionClient::result_callback, this, std::placeholders::_1); + this->action_client_->async_send_goal(goal_msg, send_goal_options); +} + +void CounterActionClient::goal_response_callback(std::shared_future::SharedPtr> future) { + auto goal_handle = future.get(); + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } +} + +void CounterActionClient::feedback_callback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) { + RCLCPP_INFO_STREAM(this->get_logger(), "Feedback get! current number is" << feedback->current_number); +} + +void CounterActionClient::result_callback(const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); + return; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); + return; + default: + RCLCPP_ERROR(this->get_logger(), "Unknown result code"); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Result get! Spend" << result.result->time_spent << " second."); + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_action/src/counter_server.cpp b/ros2_ws/src/cpp_action/src/counter_server.cpp new file mode 100644 index 0000000..4904d73 --- /dev/null +++ b/ros2_ws/src/cpp_action/src/counter_server.cpp @@ -0,0 +1,68 @@ +#include "cpp_action/counter_server.hpp" + +rclcpp_action::GoalResponse CounterActionServer::handle_goal( + const rclcpp_action::GoalUUID & uuid, + std::shared_ptr goal) { + RCLCPP_INFO(this->get_logger(), "Received goal request with target number %d", goal->target_number); + (void)uuid; + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse CounterActionServer::handle_cancel( + const std::shared_ptr> goal_handle) { + RCLCPP_INFO(this->get_logger(), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::ACCEPT; +} + +void CounterActionServer::handle_accepted(const std::shared_ptr + > goal_handle) { + using namespace std::placeholders; + rclcpp::Rate loop_rate(100); + const auto goal = goal_handle->get_goal(); + auto feedback = std::make_shared(); + auto result = std::make_shared(); + int i; + this->start_time = this->get_clock()->now().seconds(); + + for (i = 0; i < goal->target_number; ++i) { + this->current_time = this->get_clock()->now().seconds(); + // Check if there is a cancel request + if (goal_handle->is_canceling()) { + result->time_spent = this->current_time - this->start_time; + goal_handle->canceled(result); + RCLCPP_INFO(this->get_logger(), "Goal canceled"); + return; + } + // Update feedback + feedback->current_number = i; + goal_handle->publish_feedback(feedback); + RCLCPP_INFO(this->get_logger(), "Publish feedback"); + loop_rate.sleep(); + } + // Check if goal is done + if (rclcpp::ok()) { + result->time_spent = this->current_time - this->start_time; + goal_handle->succeed(result); + RCLCPP_INFO(this->get_logger(), "Goal succeeded"); + } +} + +void PrimeFinderSubscriber::calculate_primes(const std_msgs::msg::Int32::SharedPtr msg) { + int target_number = msg->data; + RCLCPP_INFO(this->get_logger(), "start counting the number of the prime number under %d", target_number); + int count = 0; + for (int i = 2; i <= target_number; i++) { + bool is_prime = true; + for (int j = 2; j * j <= i; j++) { + if (i % j == 0) { + is_prime = false; + break; + } + } + if (is_prime) + count++; + } + RCLCPP_INFO(this->get_logger(), "finish! has %d prime numbers", count); +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_action/src/main.cpp b/ros2_ws/src/cpp_action/src/main.cpp new file mode 100644 index 0000000..61d945a --- /dev/null +++ b/ros2_ws/src/cpp_action/src/main.cpp @@ -0,0 +1,30 @@ +#include +#include +#include "example_interfaces/action/counter.hpp" + +#include "cpp_action/counter_client.hpp" +#include "cpp_action/counter_server.hpp" + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + + // Intra-process Communication + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + + auto action_client = std::make_shared(options); + auto action_server = std::make_shared(options); + auto prime_finder = std::make_shared(options); + + // create multi-threaded executor + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); + + // add all nodes into one executor + executor.add_node(action_client); + executor.add_node(action_server); + executor.add_node(prime_finder); + + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/cpp_action/src/main1.cpp b/ros2_ws/src/cpp_action/src/main1.cpp new file mode 100644 index 0000000..99e49f3 --- /dev/null +++ b/ros2_ws/src/cpp_action/src/main1.cpp @@ -0,0 +1,30 @@ +#include +#include +#include "example_interfaces/action/counter.hpp" + +#include "cpp_action/counter_client.hpp" +#include "cpp_action/counter_server.hpp" + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + + // Intra-process Communication + rclcpp::NodeOptions options; + options.use_intra_process_comms(true); + + auto action_client = std::make_shared(options); + auto action_server = std::make_shared(options); + auto prime_finder = std::make_shared(options); + + // create single-thread executor + rclcpp::executors::SingleThreadedExecutor executor; + + // add all nodes into one executor + executor.add_node(action_client); + executor.add_node(action_server); + executor.add_node(prime_finder); + + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/cpp_action/src/thread_example.cpp b/ros2_ws/src/cpp_action/src/thread_example.cpp new file mode 100644 index 0000000..2b421c0 --- /dev/null +++ b/ros2_ws/src/cpp_action/src/thread_example.cpp @@ -0,0 +1,68 @@ +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +class MultiThreadedTimer { +public: + MultiThreadedTimer() : running_(true) { + start_time_ = std::chrono::steady_clock::now(); + } + + void run() { + for (int i = 0; i < 4; ++i) { + workers_.emplace_back(&MultiThreadedTimer::worker_task, this, i * 1ms); + } + } + ~MultiThreadedTimer() { + stop(); + } + + void stop() { + running_ = false; + for (auto& t : workers_) { + if (t.joinable()) { + t.join(); + } + } + } + +private: + // EventQueue + void worker_task(std::chrono::milliseconds initial_offset) { + std::this_thread::sleep_for(initial_offset); + + while (running_) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start_time_); + + if (elapsed.count() > 10000) break; + safe_print(elapsed.count()); + std::this_thread::sleep_for(1ms); + } + } + + // tackle Race Condition + void safe_print(long long ms) { + // std::lock_guard lock(print_mtx_); + std::cout << "[Thread " << std::this_thread::get_id() << "] Elapsed: " + << ms << " ms" << std::endl; + } + + std::atomic running_; + std::chrono::steady_clock::time_point start_time_; + std::mutex print_mtx_; + std::vector workers_; +}; + +int main() { + MultiThreadedTimer timer_app; + timer_app.run(); + + std::this_thread::sleep_for(11s); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_action/src/thread_example_1.cpp b/ros2_ws/src/cpp_action/src/thread_example_1.cpp new file mode 100644 index 0000000..343c132 --- /dev/null +++ b/ros2_ws/src/cpp_action/src/thread_example_1.cpp @@ -0,0 +1,67 @@ +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; + +class MultiThreadedTimer { +public: + MultiThreadedTimer() : running_(true) { + start_time_ = std::chrono::steady_clock::now(); + } + + void run() { + workers_.emplace_back(&MultiThreadedTimer::worker_task, this, 0ms); + } + + ~MultiThreadedTimer() { + stop(); + } + + void stop() { + running_ = false; + for (auto& t : workers_) { + if (t.joinable()) { + t.join(); + } + } + } + +private: + // EventQueue + void worker_task(std::chrono::milliseconds initial_offset) { + std::this_thread::sleep_for(initial_offset); + + while (running_) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start_time_); + + if (elapsed.count() > 10000) break; + safe_print(elapsed.count()); + std::this_thread::sleep_for(1ms); + } + } + + // tackle Race Condition + void safe_print(long long ms) { + std::lock_guard lock(print_mtx_); + std::cout << "[Thread " << std::this_thread::get_id() << "] Elapsed: " + << ms << " ms" << std::endl; + } + + std::atomic running_; + std::chrono::steady_clock::time_point start_time_; + std::mutex print_mtx_; + std::vector workers_; +}; + +int main() { + MultiThreadedTimer timer_app; + timer_app.run(); + + std::this_thread::sleep_for(11s); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_launch_examples/CMakeLists.txt b/ros2_ws/src/cpp_launch_examples/CMakeLists.txt new file mode 100644 index 0000000..538ffdb --- /dev/null +++ b/ros2_ws/src/cpp_launch_examples/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_launch_examples) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(example_interfaces REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/ros2_ws/src/cpp_launch_examples/config/pub_params.yaml b/ros2_ws/src/cpp_launch_examples/config/pub_params.yaml new file mode 100644 index 0000000..285036a --- /dev/null +++ b/ros2_ws/src/cpp_launch_examples/config/pub_params.yaml @@ -0,0 +1,9 @@ +cpp_pubsub: +ros__parameters: + message_prefix: "Hello from CPP" # string + publish_frequency: 2.0 # floating + +py_pubsub: +ros__parameters: + message_prefix: "Hello from Python" # string + publish_frequency: 1.0 # floating \ No newline at end of file diff --git a/ros2_ws/src/cpp_launch_examples/launch/examples_launch.py b/ros2_ws/src/cpp_launch_examples/launch/examples_launch.py new file mode 100644 index 0000000..d1f850c --- /dev/null +++ b/ros2_ws/src/cpp_launch_examples/launch/examples_launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + # C++ Pub-Sub + Node(package='cpp_pubsub', executable='talker', name='cpp_talker', output='screen'), + Node(package='cpp_pubsub', executable='listener', name='cpp_listener', output='screen'), + + # Python Pub-Sub + Node(package='py_pubsub', executable='talker', name='py_talker', output='screen'), + Node(package='py_pubsub', executable='listener', name='py_listener', output='screen'), + + # C++ Service + Node(package='cpp_srvcli', executable='add_two_ints_server', name='cpp_srv_server', output='screen'), + Node(package='cpp_srvcli', executable='add_two_ints_client', name='cpp_srv_client', output='screen'), + + # Python Service + Node(package='py_srvcli', executable='add_two_ints_server', name='py_srv_server', output='screen'), + Node(package='py_srvcli', executable='add_two_ints_client', name='py_srv_client', output='screen'), + ]) \ No newline at end of file diff --git a/ros2_ws/src/cpp_launch_examples/launch/examples_launch.xml b/ros2_ws/src/cpp_launch_examples/launch/examples_launch.xml new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/cpp_launch_examples/package.xml b/ros2_ws/src/cpp_launch_examples/package.xml new file mode 100644 index 0000000..4134bc5 --- /dev/null +++ b/ros2_ws/src/cpp_launch_examples/package.xml @@ -0,0 +1,21 @@ + + + + cpp_launch_examples + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_cmake + + rclcpp + example_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_ws/src/cpp_pubsub/CMakeLists.txt b/ros2_ws/src/cpp_pubsub/CMakeLists.txt new file mode 100644 index 0000000..480cfc0 --- /dev/null +++ b/ros2_ws/src/cpp_pubsub/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_pubsub) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# Add executables +add_executable(talker src/publisher_member_function.cpp) # 將"src/publisher_member_function.cpp"這個檔案命名為"talker"的可執行目標(executable target)，並加入編譯列表 +ament_target_dependencies(talker rclcpp std_msgs) + +add_executable(listener src/subscriber_member_function.cpp) # 將"src/publisher_member_function.cpp"這個檔案命名為"listener"的可執行目標(executable target)，並加入編譯列表 +ament_target_dependencies(listener rclcpp std_msgs) + +# Install executables +install(TARGETS + talker + listener + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/ros2_ws/src/cpp_pubsub/package.xml b/ros2_ws/src/cpp_pubsub/package.xml new file mode 100644 index 0000000..b459a05 --- /dev/null +++ b/ros2_ws/src/cpp_pubsub/package.xml @@ -0,0 +1,16 @@ + + + cpp_pubsub + 0.0.0 + C++ pub/sub example + your_name + Apache-2.0 + + ament_cmake + rclcpp + std_msgs + + + ament_cmake + + \ No newline at end of file diff --git a/ros2_ws/src/cpp_pubsub/src/publisher_member_function.cpp b/ros2_ws/src/cpp_pubsub/src/publisher_member_function.cpp new file mode 100644 index 0000000..2157f70 --- /dev/null +++ b/ros2_ws/src/cpp_pubsub/src/publisher_member_function.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a +* member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ + public: + MinimalPublisher() + : Node("minimal_publisher"), count_(0) + { + publisher_ = this->create_publisher("topic", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&MinimalPublisher::timer_callback, this)); + } + + private: + void timer_callback() + { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_pubsub/src/subscriber_member_function.cpp b/ros2_ws/src/cpp_pubsub/src/subscriber_member_function.cpp new file mode 100644 index 0000000..83345b4 --- /dev/null +++ b/ros2_ws/src/cpp_pubsub/src/subscriber_member_function.cpp @@ -0,0 +1,31 @@ +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +using std::placeholders::_1; + +class MinimalSubscriber : public rclcpp::Node +{ + public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + subscription_ = this->create_subscription( + "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + } + + private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg) const + { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + } + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_srvcli/CMakeLists.txt b/ros2_ws/src/cpp_srvcli/CMakeLists.txt new file mode 100644 index 0000000..a02395a --- /dev/null +++ b/ros2_ws/src/cpp_srvcli/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(cpp_srvcli) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(example_interfaces REQUIRED) + +add_executable(add_two_ints_server src/add_two_ints_server.cpp) +add_executable(add_two_ints_client src/add_two_ints_client.cpp) + +ament_target_dependencies(add_two_ints_server rclcpp example_interfaces) +ament_target_dependencies(add_two_ints_client rclcpp example_interfaces) + +install(TARGETS + add_two_ints_server + add_two_ints_client + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/ros2_ws/src/cpp_srvcli/package.xml b/ros2_ws/src/cpp_srvcli/package.xml new file mode 100644 index 0000000..cb95a05 --- /dev/null +++ b/ros2_ws/src/cpp_srvcli/package.xml @@ -0,0 +1,18 @@ + + + cpp_srvcli + 0.0.0 + C++ service/client example + Your Name + Apache License 2.0 + + ament_cmake + rclcpp + example_interfaces + rclcpp + example_interfaces + + + ament_cmake + + \ No newline at end of file diff --git a/ros2_ws/src/cpp_srvcli/src/add_two_ints_client.cpp b/ros2_ws/src/cpp_srvcli/src/add_two_ints_client.cpp new file mode 100644 index 0000000..65ab945 --- /dev/null +++ b/ros2_ws/src/cpp_srvcli/src/add_two_ints_client.cpp @@ -0,0 +1,32 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +using AddTwoInts = example_interfaces::srv::AddTwoInts; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + // 創一個名叫 "add_two_ints_client" 的 Node + auto node = rclcpp::Node::make_shared("add_two_ints_client"); + // 創建一個 "add_two_ints" 這個 Service 的 Client 端 + auto client = node->create_client("add_two_ints"); + + while (!client->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO(node->get_logger(), "Waiting for service..."); + } + + auto request = std::make_shared(); + request->a = 3; + request->b = 7; + + // 發送請求給 Server 端 + auto result = client->async_send_request(request); + // 等待 Server 端回傳資料 + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_INFO(node->get_logger(), "Result: %ld", result.get()->sum); + } + else { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Failed to call service add_three_doubles"); // CHANGE + } + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_srvcli/src/add_two_ints_server.cpp b/ros2_ws/src/cpp_srvcli/src/add_two_ints_server.cpp new file mode 100644 index 0000000..826e75e --- /dev/null +++ b/ros2_ws/src/cpp_srvcli/src/add_two_ints_server.cpp @@ -0,0 +1,35 @@ +#include "rclcpp/rclcpp.hpp" +#include "example_interfaces/srv/add_two_ints.hpp" + +using AddTwoInts = example_interfaces::srv::AddTwoInts; +using std::placeholders::_1; +using std::placeholders::_2; + +class AddTwoIntsServer : public rclcpp::Node { +public: + // 建構子, Node名稱叫做"add_two_ints_server" + AddTwoIntsServer() : Node("add_two_ints_server") { + // 建立 Service 通訊 + // 第一個參數是名稱（跟Topic一樣）, 第二個是bind函式，裡面放要運行的函式，後面照著打就好 + service_ = create_service( + "add_two_ints", std::bind(&AddTwoIntsServer::handle_add, this, _1, _2)); + } + +private: + // ()裡面就是固定格式 + // <>裡面放 Service 用的 interface，也就是定義通訊雙方的資料格式，可以看下方補充 + void handle_add(const std::shared_ptr request, + std::shared_ptr response) { + response->sum = request->a + request->b; + RCLCPP_INFO(this->get_logger(), "Received: %ld + %ld", request->a, request->b); + } + + rclcpp::Service::SharedPtr service_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/cpp_tf2/CMakeLists.txt b/ros2_ws/src/cpp_tf2/CMakeLists.txt new file mode 100644 index 0000000..6175c3d --- /dev/null +++ b/ros2_ws/src/cpp_tf2/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.5) +project(cpp_tf2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Include directories +include_directories( + include +) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +add_executable(frame_broadcaster src/frame_broadcaster.cpp) +ament_target_dependencies(frame_broadcaster + rclcpp + std_msgs + geometry_msgs + tf2_ros +) +add_executable(frame_listener src/frame_listener.cpp) +ament_target_dependencies(frame_listener + rclcpp + std_msgs + geometry_msgs + tf2_ros +) +add_executable(point_transformer src/point_transformer.cpp) +ament_target_dependencies(point_transformer + rclcpp + std_msgs + geometry_msgs + tf2_ros + tf2_geometry_msgs +) + +install(TARGETS + frame_broadcaster + frame_listener + point_transformer + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch/ + # rviz/ + DESTINATION share/${PROJECT_NAME}/launch +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros2_ws/src/cpp_tf2/package.xml b/ros2_ws/src/cpp_tf2/package.xml new file mode 100644 index 0000000..a7db28f --- /dev/null +++ b/ros2_ws/src/cpp_tf2/package.xml @@ -0,0 +1,27 @@ + + + + cpp_tf2 + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + rclcpp_action + rclcpp_components + geometry_msgs + tf2_ros + tf2_geometry_msgs + communicate_test + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_ws/src/cpp_tf2/src/frame_broadcaster.cpp b/ros2_ws/src/cpp_tf2/src/frame_broadcaster.cpp new file mode 100644 index 0000000..6a58508 --- /dev/null +++ b/ros2_ws/src/cpp_tf2/src/frame_broadcaster.cpp @@ -0,0 +1,43 @@ +#include +#include +#include + +class FrameBroadcaster : public rclcpp::Node { +public: + FrameBroadcaster() : Node("frame_broadcaster") { + broadcaster_ = std::make_shared(this); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&FrameBroadcaster::broadcastTransform, this) + ); + } + +private: + void broadcastTransform() { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = this->get_clock()->now(); + transform.header.frame_id = "world"; + transform.child_frame_id = "robot"; + + transform.transform.translation.x = 1.0; + transform.transform.translation.y = 2.0; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.7071; + transform.transform.rotation.w = 0.7071; + + broadcaster_->sendTransform(transform); + } + + std::shared_ptr broadcaster_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/cpp_tf2/src/frame_listener.cpp b/ros2_ws/src/cpp_tf2/src/frame_listener.cpp new file mode 100644 index 0000000..cc87d0f --- /dev/null +++ b/ros2_ws/src/cpp_tf2/src/frame_listener.cpp @@ -0,0 +1,38 @@ +#include +#include +#include +#include + +class FrameListener : public rclcpp::Node { +public: + FrameListener() : Node("frame_listener"), buffer_(this->get_clock()), listener_(buffer_) { + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&FrameListener::lookupTransform, this) + ); + } + +private: + void lookupTransform() { + try { + auto transform = buffer_.lookupTransform("world", "robot", tf2::TimePointZero); + RCLCPP_INFO(this->get_logger(), "Transform: x=%f, y=%f, z=%f", + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "Failed to lookup transform: %s", ex.what()); + } + } + + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/cpp_tf2/src/point_transformer.cpp b/ros2_ws/src/cpp_tf2/src/point_transformer.cpp new file mode 100644 index 0000000..6aeeb0a --- /dev/null +++ b/ros2_ws/src/cpp_tf2/src/point_transformer.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include +#include + +class PointTransformer : public rclcpp::Node { +public: + PointTransformer() : Node("point_transformer"), buffer_(this->get_clock()), listener_(buffer_) { + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&PointTransformer::transformPoint, this) + ); + } + +private: + void transformPoint() { + geometry_msgs::msg::PointStamped point; + point.header.frame_id = "robot"; + point.header.stamp = this->get_clock()->now(); + point.point.x = 1.0; + point.point.y = 2.0; + point.point.z = 3.0; + + try { + auto transformed_point = buffer_.transform(point, "world"); + RCLCPP_INFO(this->get_logger(), "Transformed Point: x=%f, y=%f, z=%f", + transformed_point.point.x, + transformed_point.point.y, + transformed_point.point.z); + } catch (const tf2::TransformException &ex) { + RCLCPP_WARN(this->get_logger(), "Failed to transform point: %s", ex.what()); + } + } + + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener listener_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ros2_ws/src/example_interfaces/CMakeLists.txt b/ros2_ws/src/example_interfaces/CMakeLists.txt new file mode 100644 index 0000000..7c6a3fc --- /dev/null +++ b/ros2_ws/src/example_interfaces/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.16) +project(example_interfaces) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(action_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(example_interfaces + "srv/AddTwoInts.srv" + "action/Fibonacci.action" + "action/Counter.action" + DEPENDENCIES geometry_msgs action_msgs std_msgs builtin_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/ros2_ws/src/example_interfaces/action/Counter.action b/ros2_ws/src/example_interfaces/action/Counter.action new file mode 100644 index 0000000..751d1d6 --- /dev/null +++ b/ros2_ws/src/example_interfaces/action/Counter.action @@ -0,0 +1,5 @@ +int32 target_number +--- +float64 time_spent +--- +int32 current_number \ No newline at end of file diff --git a/ros2_ws/src/example_interfaces/action/Fibonacci.action b/ros2_ws/src/example_interfaces/action/Fibonacci.action new file mode 100644 index 0000000..32b18f2 --- /dev/null +++ b/ros2_ws/src/example_interfaces/action/Fibonacci.action @@ -0,0 +1,5 @@ +int32 order +--- +int32[] sequence +--- +int32[] partial_sequence \ No newline at end of file diff --git a/ros2_ws/src/example_interfaces/package.xml b/ros2_ws/src/example_interfaces/package.xml new file mode 100644 index 0000000..a41edca --- /dev/null +++ b/ros2_ws/src/example_interfaces/package.xml @@ -0,0 +1,30 @@ + + example_interfaces + 0.2.0 + TODO: Package description + + Davide Faconti + MIT + Davide Faconti + + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rclcpp + rclcpp_action + rclcpp_components + action_msgs + std_msgs + geometry_msgs + builtin_interfaces + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/ros2_ws/src/example_interfaces/srv/AddTwoInts.srv b/ros2_ws/src/example_interfaces/srv/AddTwoInts.srv new file mode 100644 index 0000000..e717870 --- /dev/null +++ b/ros2_ws/src/example_interfaces/srv/AddTwoInts.srv @@ -0,0 +1,4 @@ +int64 a # request要送的資料 +int64 b # request要送的資料 +--- +int64 sum # response會回傳的資料 \ No newline at end of file diff --git a/ros2_ws/src/learning_tf2_cpp/CMakeLists.txt b/ros2_ws/src/learning_tf2_cpp/CMakeLists.txt new file mode 100755 index 0000000..f31064d --- /dev/null +++ b/ros2_ws/src/learning_tf2_cpp/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 3.8) +project(learning_tf2_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(turtlesim REQUIRED) + +add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp) +ament_target_dependencies( + turtle_tf2_broadcaster + geometry_msgs + rclcpp + tf2 + tf2_ros + turtlesim +) + +install(TARGETS + turtle_tf2_broadcaster + DESTINATION lib/${PROJECT_NAME}) + + +add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp) +ament_target_dependencies( + turtle_tf2_listener + geometry_msgs + rclcpp + tf2 + tf2_ros + turtlesim +) + +install(TARGETS + turtle_tf2_listener + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ros2_ws/src/learning_tf2_cpp/LICENSE b/ros2_ws/src/learning_tf2_cpp/LICENSE new file mode 100755 index 0000000..d645695 --- /dev/null +++ b/ros2_ws/src/learning_tf2_cpp/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/ros2_ws/src/learning_tf2_cpp/launch/turtle_tf2_demo.launch.py b/ros2_ws/src/learning_tf2_cpp/launch/turtle_tf2_demo.launch.py new file mode 100755 index 0000000..f096f62 --- /dev/null +++ b/ros2_ws/src/learning_tf2_cpp/launch/turtle_tf2_demo.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='turtlesim', + executable='turtlesim_node', + name='sim' + ), + Node( + package='learning_tf2_cpp', + executable='turtle_tf2_broadcaster', + name='broadcaster1', + parameters=[ + {'turtlename': 'turtle1'} + ] + ), + DeclareLaunchArgument( + 'target_frame', default_value='turtle1', + description='Target frame name.' + ), + Node( + package='learning_tf2_cpp', + executable='turtle_tf2_broadcaster', + name='broadcaster2', + parameters=[ + {'turtlename': 'turtle2'} + ] + ), + Node( + package='learning_tf2_cpp', + executable='turtle_tf2_listener', + name='listener', + parameters=[ + {'target_frame': LaunchConfiguration('target_frame')} + ] + ) + ]) \ No newline at end of file diff --git a/ros2_ws/src/learning_tf2_cpp/package.xml b/ros2_ws/src/learning_tf2_cpp/package.xml new file mode 100755 index 0000000..a78579d --- /dev/null +++ b/ros2_ws/src/learning_tf2_cpp/package.xml @@ -0,0 +1,24 @@ + + + + learning_tf2_cpp + 0.0.0 + TODO: Package description + user + Apache-2.0 + + ament_cmake + + geometry_msgs + rclcpp + tf2 + tf2_ros + turtlesim + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ros2_ws/src/learning_tf2_cpp/src/turtle_tf2_broadcaster.cpp b/ros2_ws/src/learning_tf2_cpp/src/turtle_tf2_broadcaster.cpp new file mode 100755 index 0000000..1571098 --- /dev/null +++ b/ros2_ws/src/learning_tf2_cpp/src/turtle_tf2_broadcaster.cpp @@ -0,0 +1,78 @@ +#include +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/transform_broadcaster.h" +#include "turtlesim/msg/pose.hpp" + +class FramePublisher : public rclcpp::Node +{ +public: + FramePublisher() + : Node("turtle_tf2_frame_publisher") + { + // Declare and acquire `turtlename` parameter + turtlename_ = this->declare_parameter("turtlename", "turtle"); + + // Initialize the transform broadcaster + tf_broadcaster_ = + std::make_unique(*this); + + // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose + // callback function on each message + std::ostringstream stream; + stream << "/" << turtlename_.c_str() << "/pose"; + std::string topic_name = stream.str(); + + subscription_ = this->create_subscription( + topic_name, 10, + std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1)); + } + +private: + void handle_turtle_pose(const std::shared_ptr msg) + { + geometry_msgs::msg::TransformStamped t; + + // Read message content and assign it to + // corresponding tf variables + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "map"; + t.child_frame_id = turtlename_.c_str(); + + // Turtle only exists in 2D, thus we get x and y translation + // coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = msg->x; + t.transform.translation.y = msg->y; + t.transform.translation.z = 0.0; + + // For the same reason, turtle can only rotate around one axis + // and this why we set rotation in x and y to 0 and obtain + // rotation in z axis from the message + tf2::Quaternion q; + q.setRPY(0, 0, msg->theta); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + // Send the transformation + tf_broadcaster_->sendTransform(t); + } + + rclcpp::Subscription::SharedPtr subscription_; + std::unique_ptr tf_broadcaster_; + std::string turtlename_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/learning_tf2_cpp/src/turtle_tf2_listener.cpp b/ros2_ws/src/learning_tf2_cpp/src/turtle_tf2_listener.cpp new file mode 100755 index 0000000..54693cb --- /dev/null +++ b/ros2_ws/src/learning_tf2_cpp/src/turtle_tf2_listener.cpp @@ -0,0 +1,135 @@ +#include +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "turtlesim/srv/spawn.hpp" + +using namespace std::chrono_literals; + +class FrameListener : public rclcpp::Node +{ +public: + FrameListener() + : Node("turtle_tf2_frame_listener"), + turtle_spawning_service_ready_(false), + turtle_spawned_(false) + { + // Declare and acquire `target_frame` parameter + target_frame_ = this->declare_parameter("target_frame", "turtle1"); + + tf_buffer_ = + std::make_unique(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + + // Create a client to spawn a turtle + spawner_ = + this->create_client("spawn"); + + // Create turtle2 velocity publisher + publisher_ = + this->create_publisher("turtle2/cmd_vel", 1); + + // Call on_timer function every second + timer_ = this->create_wall_timer( + 1s, std::bind(&FrameListener::on_timer, this)); + } + +private: + void on_timer() + { + // Store frame names in variables that will be used to + // compute transformations + std::string fromFrameRel = target_frame_.c_str(); + std::string toFrameRel = "turtle2"; + + if (turtle_spawning_service_ready_) { + if (turtle_spawned_) { + geometry_msgs::msg::TransformStamped t; + + // Look up for the transformation between target_frame and turtle2 frames + // and send velocity commands for turtle2 to reach target_frame + try { + t = tf_buffer_->lookupTransform( + toFrameRel, fromFrameRel, + tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform %s to %s: %s", + toFrameRel.c_str(), fromFrameRel.c_str(), ex.what()); + return; + } + + geometry_msgs::msg::Twist msg; + + static const double scaleRotationRate = 1.0; + msg.angular.z = scaleRotationRate * atan2( + t.transform.translation.y, + t.transform.translation.x); + + static const double scaleForwardSpeed = 0.5; + msg.linear.x = scaleForwardSpeed * sqrt( + pow(t.transform.translation.x, 2) + + pow(t.transform.translation.y, 2)); + + publisher_->publish(msg); + } else { + RCLCPP_INFO(this->get_logger(), "Successfully spawned"); + turtle_spawned_ = true; + } + } else { + // Check if the service is ready + if (spawner_->service_is_ready()) { + // Initialize request with turtle name and coordinates + // Note that x, y and theta are defined as floats in turtlesim/srv/Spawn + auto request = std::make_shared(); + request->x = 4.0; + request->y = 2.0; + request->theta = 0.0; + request->name = "turtle2"; + + // Call request + using ServiceResponseFuture = + rclcpp::Client::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { + auto result = future.get(); + if (strcmp(result->name.c_str(), "turtle2") == 0) { + turtle_spawning_service_ready_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch"); + } + }; + auto result = spawner_->async_send_request(request, response_received_callback); + } else { + RCLCPP_INFO(this->get_logger(), "Service is not ready"); + } + } + } + + // Boolean values to store the information + // if the service for spawning turtle is available + bool turtle_spawning_service_ready_; + // if the turtle was successfully spawned + bool turtle_spawned_; + rclcpp::Client::SharedPtr spawner_{nullptr}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + rclcpp::Publisher::SharedPtr publisher_{nullptr}; + std::shared_ptr tf_listener_{nullptr}; + std::unique_ptr tf_buffer_; + std::string target_frame_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/ros2_ws/src/py_pubsub/package.xml b/ros2_ws/src/py_pubsub/package.xml new file mode 100644 index 0000000..e8eb33b --- /dev/null +++ b/ros2_ws/src/py_pubsub/package.xml @@ -0,0 +1,15 @@ + + + py_pubsub + 0.0.0 + Python pub/sub example + ros + Apache-2.0 + + rclpy + std_msgs + + + ament_python + + \ No newline at end of file diff --git a/ros2_ws/src/py_pubsub/py_pubsub/__init__.py b/ros2_ws/src/py_pubsub/py_pubsub/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/py_pubsub/py_pubsub/publisher_member_function.py b/ros2_ws/src/py_pubsub/py_pubsub/publisher_member_function.py new file mode 100644 index 0000000..fff0ae1 --- /dev/null +++ b/ros2_ws/src/py_pubsub/py_pubsub/publisher_member_function.py @@ -0,0 +1,39 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('minimal_publisher') + self.publisher_ = self.create_publisher(String, 'topic', 10) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = String() + msg.data = 'Hello World: %d' % self.i + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/py_pubsub/py_pubsub/subscriber_member_function.py b/ros2_ws/src/py_pubsub/py_pubsub/subscriber_member_function.py new file mode 100644 index 0000000..90b6807 --- /dev/null +++ b/ros2_ws/src/py_pubsub/py_pubsub/subscriber_member_function.py @@ -0,0 +1,37 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalSubscriber(Node): + + def __init__(self): + super().__init__('minimal_subscriber') + self.subscription = self.create_subscription( + String, + 'topic', + self.listener_callback, + 10) + self.subscription # prevent unused variable warning + + def listener_callback(self, msg): + self.get_logger().info('I heard: "%s"' % msg.data) + + +def main(args=None): + rclpy.init(args=args) + + minimal_subscriber = MinimalSubscriber() + + rclpy.spin(minimal_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/py_pubsub/resource/py_pubsub b/ros2_ws/src/py_pubsub/resource/py_pubsub new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/py_pubsub/setup.cfg b/ros2_ws/src/py_pubsub/setup.cfg new file mode 100644 index 0000000..3f886b9 --- /dev/null +++ b/ros2_ws/src/py_pubsub/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_pubsub +[install] +install_scripts=$base/lib/py_pubsub diff --git a/ros2_ws/src/py_pubsub/setup.py b/ros2_ws/src/py_pubsub/setup.py new file mode 100644 index 0000000..3c6b2d0 --- /dev/null +++ b/ros2_ws/src/py_pubsub/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'py_pubsub' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + maintainer='ros', + maintainer_email='ros@todo.todo', + description='Python pub/sub example', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'talker = py_pubsub.publisher_member_function:main', + 'listener = py_pubsub.subscriber_member_function:main', + ], + }, +) diff --git a/ros2_ws/src/py_pubsub/test/test_copyright.py b/ros2_ws/src/py_pubsub/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_ws/src/py_pubsub/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/ros2_ws/src/py_pubsub/test/test_flake8.py b/ros2_ws/src/py_pubsub/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_ws/src/py_pubsub/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/ros2_ws/src/py_pubsub/test/test_pep257.py b/ros2_ws/src/py_pubsub/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_ws/src/py_pubsub/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' diff --git a/ros2_ws/src/py_srvcli/package.xml b/ros2_ws/src/py_srvcli/package.xml new file mode 100644 index 0000000..fcb6512 --- /dev/null +++ b/ros2_ws/src/py_srvcli/package.xml @@ -0,0 +1,15 @@ + + + py_srvcli + 0.0.0 + Python service example + ros + Apache-2.0 + + rclpy + std_msgs + + + ament_python + + \ No newline at end of file diff --git a/ros2_ws/src/py_srvcli/py_srvcli/__init__.py b/ros2_ws/src/py_srvcli/py_srvcli/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/py_srvcli/py_srvcli/add_two_ints_client.py b/ros2_ws/src/py_srvcli/py_srvcli/add_two_ints_client.py new file mode 100644 index 0000000..d816a09 --- /dev/null +++ b/ros2_ws/src/py_srvcli/py_srvcli/add_two_ints_client.py @@ -0,0 +1,32 @@ +import rclpy +from rclpy.node import Node +from example_interfaces.srv import AddTwoInts + +class AddTwoIntsClient(Node): + def __init__(self): + super().__init__('add_two_ints_client') + # 建立 Client 端，參數分別為： + # (使用的interface(資料格式), Service名稱) + self.client = self.create_client(AddTwoInts, 'add_two_ints') + while not self.client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waiting for service...') + self.send_request() + + def send_request(self): + req = AddTwoInts.Request() + req.a = 3 + req.b = 5 + # 發送 request + future = self.client.call_async(req) + # 等待 Server 端回傳資料 + rclpy.spin_until_future_complete(self, future) + if future.result(): + self.get_logger().info(f'Result: {future.result().sum}') + +def main(args=None): + rclpy.init(args=args) + node = AddTwoIntsClient() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/py_srvcli/py_srvcli/add_two_ints_server.py b/ros2_ws/src/py_srvcli/py_srvcli/add_two_ints_server.py new file mode 100644 index 0000000..4957045 --- /dev/null +++ b/ros2_ws/src/py_srvcli/py_srvcli/add_two_ints_server.py @@ -0,0 +1,26 @@ +import rclpy +from rclpy.node import Node +from example_interfaces.srv import AddTwoInts + +class AddTwoIntsServer(Node): + # 初始化 + def __init__(self): + # 'add_two_ints_server' 為 Node名稱，其他照著打 + super().__init__('add_two_ints_server') + # 建立 Service 端，參數分別為： + # (使用的interface(資料格式), Service名稱, 要運行的函式)，可以看一下C++版本對照一下 + self.srv = self.create_service(AddTwoInts, 'add_two_ints', self.handle_add) + + def handle_add(self, request, response): + self.get_logger().info(f"Received request: {request.a} + {request.b}") + response.sum = request.a + request.b + return response + +def main(args=None): + rclpy.init(args=args) + node = AddTwoIntsServer() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/ros2_ws/src/py_srvcli/resource/py_srvcli b/ros2_ws/src/py_srvcli/resource/py_srvcli new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/py_srvcli/setup.cfg b/ros2_ws/src/py_srvcli/setup.cfg new file mode 100644 index 0000000..3683684 --- /dev/null +++ b/ros2_ws/src/py_srvcli/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/py_srvcli +[install] +install_scripts=$base/lib/py_srvcli diff --git a/ros2_ws/src/py_srvcli/setup.py b/ros2_ws/src/py_srvcli/setup.py new file mode 100644 index 0000000..d911a70 --- /dev/null +++ b/ros2_ws/src/py_srvcli/setup.py @@ -0,0 +1,18 @@ +from setuptools import setup +package_name = 'py_srvcli' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + entry_points={ + 'console_scripts': [ + 'add_two_ints_server = py_srvcli.add_two_ints_server:main', + 'add_two_ints_client = py_srvcli.add_two_ints_client:main', + ], + }, + install_requires=['setuptools'], + zip_safe=True, + maintainer='your_name', + description='Example Service/Client in Python', +) \ No newline at end of file diff --git a/ros2_ws/src/py_srvcli/test/test_copyright.py b/ros2_ws/src/py_srvcli/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_ws/src/py_srvcli/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/ros2_ws/src/py_srvcli/test/test_flake8.py b/ros2_ws/src/py_srvcli/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_ws/src/py_srvcli/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/ros2_ws/src/py_srvcli/test/test_pep257.py b/ros2_ws/src/py_srvcli/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_ws/src/py_srvcli/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' diff --git a/ros2_ws/src/python_launch_examples/config/pub_params.yaml b/ros2_ws/src/python_launch_examples/config/pub_params.yaml new file mode 100644 index 0000000..285036a --- /dev/null +++ b/ros2_ws/src/python_launch_examples/config/pub_params.yaml @@ -0,0 +1,9 @@ +cpp_pubsub: +ros__parameters: + message_prefix: "Hello from CPP" # string + publish_frequency: 2.0 # floating + +py_pubsub: +ros__parameters: + message_prefix: "Hello from Python" # string + publish_frequency: 1.0 # floating \ No newline at end of file diff --git a/ros2_ws/src/python_launch_examples/launch/launch_arg_examples.py b/ros2_ws/src/python_launch_examples/launch/launch_arg_examples.py new file mode 100644 index 0000000..a692da8 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/launch/launch_arg_examples.py @@ -0,0 +1,41 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, LogInfo +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + # 宣告兩個 launch arguments： + # use_py_talker: bool，決定是否啟動 py_talker + # prefix: 傳給 py_talker 的 message_prefix 參數 + use_py_talker_arg = DeclareLaunchArgument( + 'use_py_talker', + default_value='true', + description='Whether to launch py_talker node' + ) + prefix_arg = DeclareLaunchArgument( + 'prefix', + default_value='Hello from launch args!', + description='Message prefix parameter for py_talker' + ) + + use_py_talker = LaunchConfiguration('use_py_talker') + prefix = LaunchConfiguration('prefix') + + # 定義節點，但包在條件中，只有 use_py_talker 是 true 時才啟動 + py_talker_node = Node( + package='py_pubsub', + executable='talker', + name='py_talker_arg', + output='screen', + parameters=[{'message_prefix': prefix}] + ) + + return LaunchDescription([ + use_py_talker_arg, + prefix_arg, + LogInfo(msg=['use_py_talker = ', use_py_talker]), + LogInfo(msg=['prefix = ', prefix]), + # 使用 Python 條件判斷來控制是否加入節點 + # LaunchDescription 不支援 if else 直接加入節點，所以用 Condition 或 Python API 動態控制 + py_talker_node if use_py_talker.perform({}) == 'true' else None + ]) \ No newline at end of file diff --git a/ros2_ws/src/python_launch_examples/launch/python_examples_launch.py b/ros2_ws/src/python_launch_examples/launch/python_examples_launch.py new file mode 100644 index 0000000..d1f850c --- /dev/null +++ b/ros2_ws/src/python_launch_examples/launch/python_examples_launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + # C++ Pub-Sub + Node(package='cpp_pubsub', executable='talker', name='cpp_talker', output='screen'), + Node(package='cpp_pubsub', executable='listener', name='cpp_listener', output='screen'), + + # Python Pub-Sub + Node(package='py_pubsub', executable='talker', name='py_talker', output='screen'), + Node(package='py_pubsub', executable='listener', name='py_listener', output='screen'), + + # C++ Service + Node(package='cpp_srvcli', executable='add_two_ints_server', name='cpp_srv_server', output='screen'), + Node(package='cpp_srvcli', executable='add_two_ints_client', name='cpp_srv_client', output='screen'), + + # Python Service + Node(package='py_srvcli', executable='add_two_ints_server', name='py_srv_server', output='screen'), + Node(package='py_srvcli', executable='add_two_ints_client', name='py_srv_client', output='screen'), + ]) \ No newline at end of file diff --git a/ros2_ws/src/python_launch_examples/launch/talker_with_params.py b/ros2_ws/src/python_launch_examples/launch/talker_with_params.py new file mode 100644 index 0000000..5a6614e --- /dev/null +++ b/ros2_ws/src/python_launch_examples/launch/talker_with_params.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='cpp_pubsub', + executable='talker', + name='cpp_talker_param', + parameters=['config/pub_params.yaml'], #參數採用config資料夾中的pub_params.yaml + output='screen' + ), + Node( + package='py_pubsub', + executable='talker', + name='py_talker_param', + parameters=['config/pub_params.yaml'], #參數採用config資料夾中的pub_params.yaml + output='screen' + ) + ]) \ No newline at end of file diff --git a/ros2_ws/src/python_launch_examples/package.xml b/ros2_ws/src/python_launch_examples/package.xml new file mode 100644 index 0000000..3db37f0 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/package.xml @@ -0,0 +1,18 @@ + + + + python_launch_examples + 0.0.0 + TODO: Package description + user + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2_ws/src/python_launch_examples/python_launch_examples/__init__.py b/ros2_ws/src/python_launch_examples/python_launch_examples/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/python_launch_examples/resource/python_launch_examples b/ros2_ws/src/python_launch_examples/resource/python_launch_examples new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/python_launch_examples/setup.cfg b/ros2_ws/src/python_launch_examples/setup.cfg new file mode 100644 index 0000000..abbf227 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/python_launch_examples +[install] +install_scripts=$base/lib/python_launch_examples diff --git a/ros2_ws/src/python_launch_examples/setup.py b/ros2_ws/src/python_launch_examples/setup.py new file mode 100644 index 0000000..24ff591 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/setup.py @@ -0,0 +1,20 @@ +from setuptools import setup + +package_name = 'python_launch_examples' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', ['launch/python_examples_launch.py']), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='your_name', + maintainer_email='your_email@example.com', + description='A launch package for pub-sub, service and action examples', + license='Apache License 2.0', +) \ No newline at end of file diff --git a/ros2_ws/src/python_launch_examples/test/test_copyright.py b/ros2_ws/src/python_launch_examples/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/ros2_ws/src/python_launch_examples/test/test_flake8.py b/ros2_ws/src/python_launch_examples/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/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/ros2_ws/src/python_launch_examples/test/test_pep257.py b/ros2_ws/src/python_launch_examples/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_ws/src/python_launch_examples/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' diff --git a/ros2_ws/src/rviz_gazebo_demo/urdf/diff_robot.urdf b/ros2_ws/src/rviz_gazebo_demo/urdf/diff_robot.urdf new file mode 100755 index 0000000..b0f1518 --- /dev/null +++ b/ros2_ws/src/rviz_gazebo_demo/urdf/diff_robot.urdf @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + left_wheel_joint + right_wheel_joint + + + 0.30 + 0.10 + + + true + true false + + + odom + base_link + + + cmd_vel + odom + + + + + diff --git a/ros2_ws/src/turtle_square_control/launch/turtle_launch.py b/ros2_ws/src/turtle_square_control/launch/turtle_launch.py new file mode 100644 index 0000000..80dd5c5 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/launch/turtle_launch.py @@ -0,0 +1,68 @@ +import os +import datetime +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import ExecuteProcess, RegisterEventHandler, EmitEvent +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown + +def generate_launch_description(): + # 取得當下時間並格式化為字串 (例如: 20260322_153025) + current_time = datetime.datetime.now().strftime("%Y%m%d_%H%M%S") + + # 組合出帶有時間戳記的資料夾名稱 + bag_folder_name = f'turtle1_status_bag_{current_time}' + + # 使用 os.path.expanduser 取得家目錄，並組合成你想要的完整路徑 + bag_path = os.path.join( + os.path.expanduser('~'), + 'ros_tutorial', 'ros2_ws', 'src', 'turtle_square_control', 'rosbag' , bag_folder_name + ) + + # 定義各個節點與執行程序 + turtlesim_node = Node( + package='turtlesim', + executable='turtlesim_node', + name='sim' + ) + + controller_node = Node( + package='turtle_square_control', + executable='controller', + name='turtle_controller', + parameters=[{ + 'start_x': 5.0, + 'start_y': 5.0, + 'square_length': 2.5 + }], + output='screen' + ) + + rqt_plot_node = Node( + package='rqt_plot', + executable='rqt_plot', + name='rqt_plot', + arguments=['/turtle1/cmd_vel/linear/x', '/turtle1/cmd_vel/angular/z'], + output='screen' + ) + + rosbag_record = ExecuteProcess( + cmd=['ros2', 'bag', 'record', '-o', bag_path, '/turtle1/cmd_vel', '/turtle1/pose'], + output='screen' + ) + + # 註冊事件處理器：當 controller_node 結束時，觸發 Shutdown 事件關閉整個 launch + shutdown_handler = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=controller_node, + on_exit=[EmitEvent(event=Shutdown())] + ) + ) + + return LaunchDescription([ + turtlesim_node, + controller_node, + rqt_plot_node, + rosbag_record, + shutdown_handler # 加入事件處理器 + ]) diff --git a/ros2_ws/src/turtle_square_control/package.xml b/ros2_ws/src/turtle_square_control/package.xml new file mode 100644 index 0000000..939f11b --- /dev/null +++ b/ros2_ws/src/turtle_square_control/package.xml @@ -0,0 +1,22 @@ + + + + turtle_square_control + 0.0.0 + TODO: Package description + user + TODO: License declaration + + rclpy + geometry_msgs + turtlesim + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ros2_ws/src/turtle_square_control/resource/turtle_square_control b/ros2_ws/src/turtle_square_control/resource/turtle_square_control new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/turtle_square_control/rosbag/turtle1_status_bag_20260325_121031/metadata.yaml b/ros2_ws/src/turtle_square_control/rosbag/turtle1_status_bag_20260325_121031/metadata.yaml new file mode 100644 index 0000000..d8a18f5 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/rosbag/turtle1_status_bag_20260325_121031/metadata.yaml @@ -0,0 +1,32 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 17609813087 + starting_time: + nanoseconds_since_epoch: 1774411831903992434 + message_count: 1443 + topics_with_message_count: + - topic_metadata: + name: /turtle1/pose + type: turtlesim/msg/Pose + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 1094 + - topic_metadata: + name: /turtle1/cmd_vel + type: geometry_msgs/msg/Twist + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 349 + compression_format: "" + compression_mode: "" + relative_file_paths: + - turtle1_status_bag_20260325_121031_0.db3 + files: + - path: turtle1_status_bag_20260325_121031_0.db3 + starting_time: + nanoseconds_since_epoch: 1774411831903992434 + duration: + nanoseconds: 17609813087 + message_count: 1443 \ No newline at end of file diff --git a/ros2_ws/src/turtle_square_control/rosbag/turtle1_status_bag_20260325_121031/turtle1_status_bag_20260325_121031_0.db3 b/ros2_ws/src/turtle_square_control/rosbag/turtle1_status_bag_20260325_121031/turtle1_status_bag_20260325_121031_0.db3 new file mode 100644 index 0000000..fe6a638 Binary files /dev/null and b/ros2_ws/src/turtle_square_control/rosbag/turtle1_status_bag_20260325_121031/turtle1_status_bag_20260325_121031_0.db3 differ diff --git a/ros2_ws/src/turtle_square_control/setup.cfg b/ros2_ws/src/turtle_square_control/setup.cfg new file mode 100644 index 0000000..58b1590 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/turtle_square_control +[install] +install_scripts=$base/lib/turtle_square_control diff --git a/ros2_ws/src/turtle_square_control/setup.py b/ros2_ws/src/turtle_square_control/setup.py new file mode 100644 index 0000000..5beeed3 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/setup.py @@ -0,0 +1,33 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'turtle_square_control' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='user', + maintainer_email='user@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'controller = turtle_square_control.turtle_controller:main' + ], + }, +) diff --git a/ros2_ws/src/turtle_square_control/test/test_copyright.py b/ros2_ws/src/turtle_square_control/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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/ros2_ws/src/turtle_square_control/test/test_flake8.py b/ros2_ws/src/turtle_square_control/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/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/ros2_ws/src/turtle_square_control/test/test_pep257.py b/ros2_ws/src/turtle_square_control/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/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' diff --git a/ros2_ws/src/turtle_square_control/turtle_square_control/__init__.py b/ros2_ws/src/turtle_square_control/turtle_square_control/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ros2_ws/src/turtle_square_control/turtle_square_control/turtle_controller.py b/ros2_ws/src/turtle_square_control/turtle_square_control/turtle_controller.py new file mode 100644 index 0000000..a7bfec0 --- /dev/null +++ b/ros2_ws/src/turtle_square_control/turtle_square_control/turtle_controller.py @@ -0,0 +1,353 @@ +import sys # 新增 sys 模組以便呼叫 sys.exit() +import math +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from turtlesim.msg import Pose +from turtlesim.srv import Spawn, TeleportAbsolute, SetPen + + +class TurtleController(Node): + def __init__(self): + super().__init__('turtle_controller') + + self.declare_parameter('start_x', 5.0) + self.declare_parameter('start_y', 5.0) + self.declare_parameter('square_length', 2.5) + + self.start_x = self.get_parameter('start_x').value + self.start_y = self.get_parameter('start_y').value + self.square_length = self.get_parameter('square_length').value + + self.cmd_vel_pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10) + self.pose_sub = self.create_subscription( + Pose, '/turtle1/pose', self.pose_callback, 10) + + self.spawn_client = self.create_client(Spawn, '/spawn') + self.teleport_client = self.create_client( + TeleportAbsolute, '/turtle1/teleport_absolute') + self.pen_client = self.create_client(SetPen, '/turtle1/set_pen') + + self.current_pose = None + self.state = 'INIT_PEN_OFF' + self.edges_completed = 0 + self.child_turtles = {} + self.wait_ticks = 0 + + self.waypoints = [ + (self.start_x + self.square_length, self.start_y), + (self.start_x + self.square_length, self.start_y - self.square_length), + (self.start_x, self.start_y - self.square_length), + (self.start_x, self.start_y) + ] + self.target_headings = [0.0, -math.pi / 2, math.pi, math.pi / 2] + + self.angular_stop_thresh = 0.005 + self.linear_ramp_dist = 0.3 + self.high_linear_vel = 2.0 + self.low_linear_vel = 0.1 + + self.timer = self.create_timer(0.05, self.control_loop) + self.get_logger().info('Controller started.') + + # ------------------------------------------------------------------ # + + def pose_callback(self, msg): + self.current_pose = msg + + def normalize_angle(self, angle): + return math.atan2(math.sin(angle), math.cos(angle)) + + # ------------------------------------------------------------------ # + # Child turtle setup # + # ------------------------------------------------------------------ # + + def setup_child_turtle(self, name, corner_x, corner_y): + # --- Pen off --- + pen_client = self.create_client(SetPen, f'/{name}/set_pen') + if pen_client.wait_for_service(timeout_sec=1.0): + pen_client.call_async(SetPen.Request(off=1)) + else: + self.get_logger().warn(f'set_pen service not ready for {name}') + + # --- Publisher / subscriber --- + pub = self.create_publisher(Twist, f'/{name}/cmd_vel', 10) + sub = self.create_subscription( + Pose, f'/{name}/pose', + lambda msg, n=name: self.child_pose_callback(msg, n), + 10 + ) + + # Register entry with movement LOCKED until teleport confirmed + self.child_turtles[name] = { + 'pub': pub, + 'sub': sub, + 'pose': None, + 'corner_x': float(corner_x), + 'corner_y': float(corner_y), + 'teleported': False, # ← LOCK + } + + # --- Teleport to exact corner --- + teleport_client = self.create_client( + TeleportAbsolute, f'/{name}/teleport_absolute') + if teleport_client.wait_for_service(timeout_sec=1.0): + req = TeleportAbsolute.Request( + x=float(corner_x), + y=float(corner_y), + theta=0.0 + ) + future = teleport_client.call_async(req) + # UNLOCK only after the service confirms teleport is done + future.add_done_callback( + lambda f, n=name: self._on_teleport_done(f, n) + ) + self.get_logger().info( + f'{name} teleporting to exact corner ' + f'({corner_x:.3f}, {corner_y:.3f}) — movement locked.') + else: + # Fallback: unlock immediately to avoid permanent deadlock + self.child_turtles[name]['teleported'] = True + self.get_logger().warn( + f'teleport_absolute not ready for {name}; ' + f'movement unlocked without positional correction.') + + def _on_teleport_done(self, future, name): + """Teleport service responded — safe to start navigating.""" + if name in self.child_turtles: + self.child_turtles[name]['teleported'] = True # ← UNLOCK + self.get_logger().info( + f'{name} teleport confirmed — navigation unlocked.') + + def child_pose_callback(self, msg, name): + if name in self.child_turtles: + self.child_turtles[name]['pose'] = msg + + # ------------------------------------------------------------------ # + # Child turtle navigation # + # ------------------------------------------------------------------ # + + def control_children(self): + for name, data in self.child_turtles.items(): + if name == 'turtle5': + continue + + # KEY GUARD: skip until teleport is confirmed. + if not data.get('teleported', False): + continue + + pose = data['pose'] + if pose is None: + continue + + dist = math.hypot(self.start_x - pose.x, self.start_y - pose.y) + desired_h = math.atan2( + self.start_y - pose.y, self.start_x - pose.x) + h_err = self.normalize_angle(desired_h - pose.theta) + + cmd = Twist() + if dist > 0.05: + cmd.angular.z = 2.0 * h_err + # Strict heading threshold (0.02 rad) keeps paths straight + if abs(h_err) > 0.02: + cmd.linear.x = 0.0 + else: + cmd.linear.x = min(1.5, dist * 1.5) + else: + cmd.linear.x = 0.0 + cmd.angular.z = 0.0 + + data['pub'].publish(cmd) + + # ------------------------------------------------------------------ # + # Main control loop # + # ------------------------------------------------------------------ # + + def control_loop(self): + if self.current_pose is None: + return + + self.control_children() + cmd = Twist() + + # --- Initialization Phase --- + if self.state == 'INIT_PEN_OFF': + if self.pen_client.wait_for_service(timeout_sec=0.1): + self.pen_client.call_async(SetPen.Request(off=1)) + self.state = 'INIT_TELEPORT' + + elif self.state == 'INIT_TELEPORT': + if self.teleport_client.wait_for_service(timeout_sec=0.1): + self.teleport_client.call_async( + TeleportAbsolute.Request( + x=self.start_x, y=self.start_y, theta=0.0)) + self.state = 'INIT_WAIT' + + elif self.state == 'INIT_WAIT': + if (abs(self.current_pose.x - self.start_x) < 0.05 and + abs(self.current_pose.y - self.start_y) < 0.05): + self.state = 'INIT_PEN_ON' + + elif self.state == 'INIT_PEN_ON': + self.pen_client.call_async( + SetPen.Request(off=0, r=255, g=255, b=255, width=3)) + self.edges_completed = 0 + self.state = 'FORWARD' + self.get_logger().info( + 'Teleported to start. Beginning square pattern.') + + # --- Linear Movement --- + elif self.state == 'FORWARD': + target_x, target_y = self.waypoints[self.edges_completed] + + if self.edges_completed == 0: + dist_remaining = target_x - self.current_pose.x + elif self.edges_completed == 1: + dist_remaining = self.current_pose.y - target_y + elif self.edges_completed == 2: + dist_remaining = self.current_pose.x - target_x + elif self.edges_completed == 3: + dist_remaining = target_y - self.current_pose.y + + if dist_remaining > 0: + dist_to_target = math.hypot( + target_x - self.current_pose.x, + target_y - self.current_pose.y) + if dist_to_target > 0.05: + desired_heading = math.atan2( + target_y - self.current_pose.y, + target_x - self.current_pose.x) + else: + desired_heading = self.target_headings[self.edges_completed] + + h_err = self.normalize_angle( + desired_heading - self.current_pose.theta) + cmd.angular.z = 2.0 * h_err + + if dist_remaining > self.linear_ramp_dist: + cmd.linear.x = self.high_linear_vel + else: + speed_ratio = dist_remaining / self.linear_ramp_dist + cmd.linear.x = max( + self.low_linear_vel + + (self.high_linear_vel - self.low_linear_vel) * speed_ratio, + self.low_linear_vel) + else: + cmd.linear.x = 0.0 + cmd.angular.z = 0.0 + self.wait_ticks = 0 + self.state = 'WAIT_STOP_LINEAR' + + # --- Linear Braking / Wait & Absolute Spawning --- + elif self.state == 'WAIT_STOP_LINEAR': + cmd.linear.x = 0.0 + cmd.angular.z = 0.0 + self.wait_ticks += 1 + if self.wait_ticks >= 5: + turtle_name = f'turtle{self.edges_completed + 2}' + target_x, target_y = self.waypoints[self.edges_completed] + + if self.spawn_client.wait_for_service(timeout_sec=1.0): + req = Spawn.Request( + x=float(target_x), + y=float(target_y), + theta=float(self.current_pose.theta), + name=turtle_name + ) + future = self.spawn_client.call_async(req) + future.add_done_callback( + lambda f, n=turtle_name, cx=target_x, cy=target_y: + self.spawn_done_callback(f, n, cx, cy) + ) + self.state = 'WAITING_FOR_SPAWN' + self.get_logger().info( + f'Stationary. Spawning {turtle_name} at ' + f'({target_x:.3f}, {target_y:.3f}).') + else: + self.get_logger().error('Failed to call /spawn service.') + + elif self.state == 'WAITING_FOR_SPAWN': + cmd.linear.x = 0.0 + cmd.angular.z = 0.0 + + # --- Turning Movement --- + elif self.state == 'TURN': + target_heading = self.target_headings[self.edges_completed] + h_err = self.normalize_angle( + target_heading - self.current_pose.theta) + + if abs(h_err) > self.angular_stop_thresh: + cmd.angular.z = 2.5 * h_err + if cmd.angular.z > 0: + cmd.angular.z = max(min(cmd.angular.z, 1.5), 0.1) + else: + cmd.angular.z = min(max(cmd.angular.z, -1.5), -0.1) + else: + cmd.angular.z = 0.0 + self.wait_ticks = 0 + self.state = 'WAIT_STOP_ANGULAR' + + # --- Angular Braking / Wait --- + elif self.state == 'WAIT_STOP_ANGULAR': + cmd.linear.x = 0.0 + cmd.angular.z = 0.0 + self.wait_ticks += 1 + if self.wait_ticks >= 5: + self.state = 'FORWARD' + self.get_logger().info( + 'Rotation stabilized. Proceeding to next edge.') + + # --- Task Completed --- + elif self.state == 'STOP': + cmd.linear.x = 0.0 + cmd.angular.z = 0.0 + + self.cmd_vel_pub.publish(cmd) + + # ------------------------------------------------------------------ # + # Spawn callback # + # ------------------------------------------------------------------ # + + def spawn_done_callback(self, future, name, corner_x, corner_y): + self.edges_completed += 1 + + if self.edges_completed >= 4: + self.state = 'STOP' + self.get_logger().info('Square closed. 5 turtles active. Auto-shutting down launch in 3 seconds...') + # 設定 3 秒後觸發自我結束 + self.create_timer(3.0, self.shutdown_node) + else: + self.setup_child_turtle(name, corner_x, corner_y) + self.state = 'TURN' + self.get_logger().info('Spawn successful. Rotating...') + + # ------------------------------------------------------------------ # + # Shutdown Function # + # ------------------------------------------------------------------ # + def shutdown_node(self): + """結束目前 Python 行程，以觸發 launch 的 OnProcessExit""" + self.get_logger().info('Mission complete. Exiting controller node...') + sys.exit(0) + + +# ---------------------------------------------------------------------- # + +def main(args=None): + rclpy.init(args=args) + node = TurtleController() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + except SystemExit: + # 捕捉 sys.exit() 以確保優雅關閉 + pass + finally: + node.destroy_node() + # 檢查 rclpy 是否已經 shutdown，避免重複 shutdown 產生錯誤 + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/communication/CMakeLists.txt b/src/communication/CMakeLists.txt deleted file mode 100644 index 6032742..0000000 --- a/src/communication/CMakeLists.txt +++ /dev/null @@ -1,209 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(communication) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES communication -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/communication.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/communication_node.cpp) - -add_executable(stm_communication src/stm_communication.cpp) -target_link_libraries(stm_communication ${catkin_LIBRARIES}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_communication.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/communication/package.xml b/src/communication/package.xml deleted file mode 100644 index dbbe5a5..0000000 --- a/src/communication/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - communication - 0.0.0 - The communication package - - - - - user - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/src/communication/src/stm_communication.cpp b/src/communication/src/stm_communication.cpp deleted file mode 100644 index e6620b2..0000000 --- a/src/communication/src/stm_communication.cpp +++ /dev/null @@ -1,26 +0,0 @@ -//stm_communication.cpp -#include -#include "ros/ros.h" -#include - -using namespace std; - -int main(int argc, char** argv){ - ros::init(argc, argv, "stm_communication"); - ros::NodeHandle nh; - - ros::Publisher pub = nh.advertise("counting", 1); - - std_msgs::Int64 hello; - hello.data = 0; - - while(ros::ok()){ - pub.publish(hello); - hello.data++; - ROS_INFO("publish %d", hello.data); - ros::Duration(0.5).sleep(); - } - - ROS_INFO("== Terminate ! =="); - return 0; -} diff --git a/src/message/CMakeLists.txt b/src/message/CMakeLists.txt deleted file mode 100755 index 4de6791..0000000 --- a/src/message/CMakeLists.txt +++ /dev/null @@ -1,221 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(message) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - roscpp - rospy - std_msgs - turtlesim - message_generation -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - example.msg -) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES beginner_tutorials -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib - CATKIN_DEPENDS roscpp rospy std_msgs message_runtime -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/message.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/message_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_executable(day2_talker src/day2_talker.cpp) -target_link_libraries(day2_talker ${catkin_LIBRARIES}) -add_executable(day2_listener src/day2_listener.cpp) -target_link_libraries(day2_listener ${catkin_LIBRARIES}) -add_executable(turtle_homework src/turtle_homework.cpp) -target_link_libraries(turtle_homework ${catkin_LIBRARIES}) - -add_executable(day3_practice_talker src/day3_practice_talker.cpp) -target_link_libraries(day3_practice_talker ${catkin_LIBRARIES}) -add_dependencies(day3_practice_talker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -add_executable(day3_practice_listener src/day3_practice_listener.cpp) -target_link_libraries(day3_practice_listener ${catkin_LIBRARIES}) -add_dependencies(day3_practice_listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -install(PROGRAMS - scripts/talker.py - scripts/listener.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS day2_talker - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_message.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/message/msg/example.msg b/src/message/msg/example.msg deleted file mode 100644 index 7ac72a8..0000000 --- a/src/message/msg/example.msg +++ /dev/null @@ -1,3 +0,0 @@ -int64 id -string name -uint16 age \ No newline at end of file diff --git a/src/message/package.xml b/src/message/package.xml deleted file mode 100755 index 3878571..0000000 --- a/src/message/package.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - message - 0.0.0 - The message package - - - - - root - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - message_generation - - - - - - message_runtime - - - - - catkin - geometry_msgs - roscpp - rospy - std_msgs - turtlesim - geometry_msgs - roscpp - rospy - std_msgs - turtlesim - geometry_msgs - roscpp - rospy - std_msgs - turtlesim - - - - - - - - diff --git a/src/message/scripts/listener.py b/src/message/scripts/listener.py deleted file mode 100644 index 2c919ed..0000000 --- a/src/message/scripts/listener.py +++ /dev/null @@ -1,19 +0,0 @@ -#!/usr/bin/env python3 -#coding:utf-8 - -import rospy -from std_msgs.msg import Int64 - -def callbackfunc(msg): - rospy.loginfo("%d\n",msg.data) - -def subscriber(): - rospy.init_node('listener') - rospy.Subscriber('counter', Int64, callbackfunc) - rospy.spin() - -if __name__=='__main__': - try: - subscriber() - except rospy.ROSInterruptException: - pass diff --git a/src/message/scripts/talker.py b/src/message/scripts/talker.py deleted file mode 100644 index c1eb0d5..0000000 --- a/src/message/scripts/talker.py +++ /dev/null @@ -1,18 +0,0 @@ -# !/usr/bin/env python3 -# coding:utf-8 -import rospy -from std_msgs.msg import Int64 -def publisher(): - rospy.init_node('talker') - pub = rospy.Publisher('counter', Int64, queue_size = 10) - count = 0 - while not rospy.is_shutdown(): - pub.publish(count) - rospy.sleep(1) - count+=1 - -if __name__=='__main__': - try: - publisher() - except rospy.ROSInterruptException: - pass diff --git a/src/message/src/a.in b/src/message/src/a.in deleted file mode 100644 index c37dc67..0000000 --- a/src/message/src/a.in +++ /dev/null @@ -1,6 +0,0 @@ -114000730 Sean 20 -114000731 Sing 80 -114000732 Seen 64 -114000733 Sea 57 -114000734 Soon 3 -114000735 San 19 \ No newline at end of file diff --git a/src/message/src/day2_listener.cpp b/src/message/src/day2_listener.cpp deleted file mode 100644 index 85b231b..0000000 --- a/src/message/src/day2_listener.cpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/Int64.h" - -void Callback(const std_msgs::Int64::ConstPtr& msg) -{ - std::cout<<"msg.data = "<data<<"\n"; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "listener"); - ros::NodeHandle nh; - ros::Subscriber sub = nh.subscribe("counter",10,Callback); - ros::Rate loop_rate(1); - - while(ros::ok()) - { - ros::spinOnce(); - loop_rate.sleep(); - } - return 0; -} diff --git a/src/message/src/day2_talker.cpp b/src/message/src/day2_talker.cpp deleted file mode 100644 index 4d94a5e..0000000 --- a/src/message/src/day2_talker.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/Int64.h" - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "talker"); - ros::NodeHandle nh; - ros::Publisher pub = nh.advertise("counter", 10); - ros::Rate loop_rate(1); - - int number = 0; - std_msgs::Int64 msg; - - while(ros::ok()) - { - msg.data = number; - // ROS_INFO("%ld", msg.data); - pub.publish(msg); - - number++; - loop_rate.sleep(); - } - return 0; -} diff --git a/src/message/src/day3_practice_listener.cpp b/src/message/src/day3_practice_listener.cpp deleted file mode 100644 index 67d9359..0000000 --- a/src/message/src/day3_practice_listener.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/Int64.h" -#include "message/example.h" - -void Callback(const message::example::ConstPtr& msg) -{ - printf("(%ld, %s, %d)\n",msg->id,msg->name.c_str(),msg->age); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "listener"); - ros::NodeHandle nh; - ros::Subscriber sub = nh.subscribe("counter",10,Callback); - ros::Rate loop_rate(1); - - while(ros::ok()) - { - ros::spinOnce(); - loop_rate.sleep(); - } - return 0; -} diff --git a/src/message/src/day3_practice_talker.cpp b/src/message/src/day3_practice_talker.cpp deleted file mode 100644 index de3f19e..0000000 --- a/src/message/src/day3_practice_talker.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/Int64.h" -#include "message/example.h" -#include - -typedef struct student{ - int id; - std::string name; - int age; -}st; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "talker"); - ros::NodeHandle nh; - ros::Publisher pub = nh.advertise("counter", 10); - ros::Rate loop_rate(1); - - message::example msg; - std::queue stlist; - st tmp; - while(std::cin>>tmp.id>>tmp.name>>tmp.age){ - stlist.push(tmp); - } - while(ros::ok() && !stlist.empty()) - { - msg.age = stlist.front().age; - msg.name = stlist.front().name; - msg.id = stlist.front().id; - stlist.pop(); - // ROS_INFO("%ld", msg.data); - pub.publish(msg); - - loop_rate.sleep(); - } - return 0; -} diff --git a/src/message/src/turtle_homework.cpp b/src/message/src/turtle_homework.cpp deleted file mode 100644 index 5fdd40a..0000000 --- a/src/message/src/turtle_homework.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include -#include -#include - - -turtlesim::Pose current_pose; - -void callback(const turtlesim::Pose::ConstPtr& pose_data) -{ - std::cout<<"current pose = "<<*pose_data<<"\n"; - current_pose = *pose_data; -} - -int main(int argc, char **argv) -{ - ros::init(argc,argv,"move_control"); - ros::NodeHandle nh; - ros::Publisher vel_pub = nh.advertise("/turtle1/cmd_vel",1); - ros::Subscriber pose_sub = nh.subscribe("/turtle1/pose",10,callback); - ros::Rate rate(10); - geometry_msgs::Twist vel_msg; - - ros::Rate loop_rate(10); - while(ros::ok() && current_pose.x<= 9.5) - { - vel_msg.linear.x = 2; - vel_msg.angular.z = 0.0; - vel_pub.publish(vel_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - while(ros::ok() && current_pose.x<= 10) - { - vel_msg.linear.x = 0.1; - vel_msg.angular.z = 0.0; - vel_pub.publish(vel_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - while(ros::ok() && current_pose.theta>=-1.4) - { - vel_msg.linear.x = 0.0; - vel_msg.angular.z = -0.5; - vel_pub.publish(vel_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - while(ros::ok() && current_pose.theta>=-1.57079) - { - vel_msg.linear.x = 0.0; - vel_msg.angular.z = -0.02; - vel_pub.publish(vel_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - while(ros::ok() && current_pose.y>=0.5) - { - vel_msg.linear.x = 2; - vel_msg.angular.z = 0.0; - vel_pub.publish(vel_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - while(ros::ok() && current_pose.y>0) - { - vel_msg.linear.x = 0.1; - vel_msg.angular.z = 0.0; - vel_pub.publish(vel_msg); - ros::spinOnce(); - loop_rate.sleep(); - } - return 0; -} \ No newline at end of file diff --git a/src/oop/CMakeLists.txt b/src/oop/CMakeLists.txt deleted file mode 100755 index b3d306b..0000000 --- a/src/oop/CMakeLists.txt +++ /dev/null @@ -1,216 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(oop) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES oop -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -) -add_library(chassis src/chassis.cpp) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/oop.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/oop_node.cpp) - -add_executable(oop_ex1 src/oop_ex1.cpp) -target_link_libraries(oop_ex1 ${catkin_LIBRARIES}) - -add_executable(oop_ex2 src/oop_ex2.cpp) -target_link_libraries(oop_ex2 ${catkin_LIBRARIES}) - -add_executable(oop_ex3 src/oop_ex3.cpp) -target_link_libraries(oop_ex3 ${catkin_LIBRARIES}) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_oop.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/oop/include/chassis.h b/src/oop/include/chassis.h deleted file mode 100755 index dd648b0..0000000 --- a/src/oop/include/chassis.h +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include - -#ifndef CHASSIS_H -#define CHASSIS_H - -class Chassis{ - public: - Chassis(float x_0, float y_0, float theta_0, float v_wheels_0[2]); - void forwardKinematics(); - void poseUpdate(); - void output(); - - private: - float x; - float y; - float theta; - float v_x=0.0; - float v_y=0.0; - float omega=0.0; - float v_wheels[2]={0.0, 0.0}; - float radius=100; - float dt=0.01; - float time=0.0; -}; - -#endif \ No newline at end of file diff --git a/src/oop/package.xml b/src/oop/package.xml deleted file mode 100755 index f4911e8..0000000 --- a/src/oop/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - oop - 0.0.0 - The oop package - - - - - user - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/src/oop/src/chassis.cpp b/src/oop/src/chassis.cpp deleted file mode 100755 index b1621bb..0000000 --- a/src/oop/src/chassis.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "chassis.h" - -Chassis::Chassis(float x_0, float y_0, float theta_0, float v_wheels_0[2]) - :x(x_0), y(y_0), theta(theta_0){ - v_wheels[0]=v_wheels_0[0]; - v_wheels[1]=v_wheels_0[1]; - } - -void Chassis::forwardKinematics(){ - v_x=(v_wheels[0]+v_wheels[1])*cos(theta)/2; - v_y=(v_wheels[0]+v_wheels[1])*sin(theta)/2; - omega=(v_wheels[0]+v_wheels[1])/(radius*2); - } - -void Chassis::poseUpdate(){ - time+=dt; - x+=v_x*dt; - y+=v_y*dt; - theta+=omega*dt; - } - -void Chassis::output(){ - std::cout<<"at "< -#include - -class Chassis{ - public: - Chassis(float x_0, float y_0, float theta_0, float v_wheels_0[2]) - :x(x_0), y(y_0), theta(theta_0){ - v_wheels[0]=v_wheels_0[0]; - v_wheels[1]=v_wheels_0[1]; - } - - void forwardKinematics(){ - v_x=(v_wheels[0]+v_wheels[1])*cos(theta)/2; - v_y=(v_wheels[0]+v_wheels[1])*sin(theta)/2; - omega=(v_wheels[0]+v_wheels[1])/(radius*2); - } - - void poseUpdate(){ - time+=dt; - x+=v_x*dt; - y+=v_y*dt; - theta+=omega*dt; - } - void output(){ - std::cout<<"at "< -#include - -class Chassis{ - public: - Chassis(float x_0, float y_0, float theta_0) - :x(x_0), y(y_0), theta(theta_0){} - - virtual void forwardKinematics()=0; - - void poseUpdate(){ - time+=dt; - x+=v_x*dt; - y+=v_y*dt; - theta+=omega*dt; - } - void output(){ - std::cout<<"at "<`_) -* Contributors: Mike Purvis - -0.8.0 (2018-10-11) ------------------- - -0.7.7 (2017-11-29) ------------------- -* Fix catkin lint errors (`#296 `_) -* Contributors: Bei Chen Liu - -0.7.6 (2017-03-01) ------------------- - -0.7.5 (2016-11-22) ------------------- - -0.7.4 (2016-09-21) ------------------- - -0.7.3 (2016-08-05) ------------------- - -0.7.2 (2016-07-15) ------------------- - -0.7.1 (2015-07-06) ------------------- - -0.7.0 (2015-04-23) ------------------- - -0.6.3 (2014-11-05) ------------------- - -0.6.2 (2014-09-10) ------------------- - -0.6.1 (2014-06-30) ------------------- - -0.6.0 (2014-06-11) ------------------- -* Uncomment ID_TX_STOP constant, per `#111 `_ -* Contributors: Mike Purvis - -0.5.6 (2014-06-11) ------------------- -* Add Mike Purvis as maintainer to all but xbee. -* remove ID_TX_STOP from rosserial_msgs/msg/TopicInfo.msg, using hardcode modification. fix the dupilcated registration problem of subscriber -* modified rosserial -* Contributors: Mike Purvis, Moju Zhao - -0.5.5 (2014-01-14) ------------------- - -0.5.4 (2013-10-17) ------------------- - -0.5.3 (2013-09-21) ------------------- -* Add message info service and stub of node to provide it, for -rosserial_server support. - -0.5.2 (2013-07-17) ------------------- - -* Fix release version - -0.5.1 (2013-07-15) ------------------- - -0.4.5 (2013-07-02) ------------------- - -0.4.4 (2013-03-20) ------------------- - -0.4.3 (2013-03-13 14:08) ------------------------- - -0.4.2 (2013-03-13 01:15) ------------------------- - -0.4.1 (2013-03-09) ------------------- - -0.4.0 (2013-03-08) ------------------- -* Changed DEBUG log level to ROSDEBUG. -* initial catkin version on github diff --git a/src/rosserial_msgs/CMakeLists.txt b/src/rosserial_msgs/CMakeLists.txt deleted file mode 100644 index c525612..0000000 --- a/src/rosserial_msgs/CMakeLists.txt +++ /dev/null @@ -1,21 +0,0 @@ -cmake_minimum_required(VERSION 3.7.2) -project(rosserial_msgs) - -find_package(catkin REQUIRED COMPONENTS - message_generation -) - -add_message_files(FILES - Log.msg - TopicInfo.msg -) - -add_service_files(FILES - RequestParam.srv -) - -generate_messages() - -catkin_package(CATKIN_DEPENDS - message_runtime -) diff --git a/src/rosserial_msgs/msg/Log.msg b/src/rosserial_msgs/msg/Log.msg deleted file mode 100644 index 7239876..0000000 --- a/src/rosserial_msgs/msg/Log.msg +++ /dev/null @@ -1,10 +0,0 @@ - -#ROS Logging Levels -uint8 ROSDEBUG=0 -uint8 INFO=1 -uint8 WARN=2 -uint8 ERROR=3 -uint8 FATAL=4 - -uint8 level -string msg diff --git a/src/rosserial_msgs/msg/TopicInfo.msg b/src/rosserial_msgs/msg/TopicInfo.msg deleted file mode 100644 index dafd6b0..0000000 --- a/src/rosserial_msgs/msg/TopicInfo.msg +++ /dev/null @@ -1,21 +0,0 @@ -# special topic_ids -uint16 ID_PUBLISHER=0 -uint16 ID_SUBSCRIBER=1 -uint16 ID_SERVICE_SERVER=2 -uint16 ID_SERVICE_CLIENT=4 -uint16 ID_PARAMETER_REQUEST=6 -uint16 ID_LOG=7 -uint16 ID_TIME=10 -uint16 ID_TX_STOP=11 - -# The endpoint ID for this topic -uint16 topic_id - -string topic_name -string message_type - -# MD5 checksum for this message type -string md5sum - -# size of the buffer message must fit in -int32 buffer_size diff --git a/src/rosserial_msgs/package.xml b/src/rosserial_msgs/package.xml deleted file mode 100644 index 9f8ac6d..0000000 --- a/src/rosserial_msgs/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - rosserial_msgs - 0.9.2 - - Messages for automatic topic configuration using rosserial. - - Michael Ferguson - Paul Bouchier - Mike Purvis - BSD - http://ros.org/wiki/rosserial_msgs - - catkin - - message_generation - - message_runtime - diff --git a/src/rosserial_msgs/srv/RequestParam.srv b/src/rosserial_msgs/srv/RequestParam.srv deleted file mode 100644 index ca605e8..0000000 --- a/src/rosserial_msgs/srv/RequestParam.srv +++ /dev/null @@ -1,7 +0,0 @@ -string name - ---- - -int32[] ints -float32[] floats -string[] strings diff --git a/src/rosserial_python/CHANGELOG.rst b/src/rosserial_python/CHANGELOG.rst deleted file mode 100644 index 28a2bf3..0000000 --- a/src/rosserial_python/CHANGELOG.rst +++ /dev/null @@ -1,175 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rosserial_python -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.9.2 (2021-04-02) ------------------- -* Adds a shutdown hook for sending txStopRequest (`#551 `_) -* Gracefully handle socket server end. (`#550 `_) -* Contributors: Can Altineller, Karl Kangur - -0.9.1 (2020-09-09) ------------------- -* rosserial_python py3 fix (`#514 `_) -* Contributors: BriceRenaudeau - -0.9.0 (2020-08-25) ------------------- -* Python 3 and GCC7+ fixes (`#508 `_) - * Port of rosserial_python to py3. - * Throw from inside the BrokenPipeError. -* Fix Travis for Noetic + Python 3 -* Bump minimum CMake version to 3.7.2 (Melodic). -* Update pyserial rosdep. -* Use time.sleep instead of rospy.sleep. (`#489 `_) -* Make deprecation message a warning and more specific (`#479 `_) -* Properly initialize message_info stub, drop from test. -* Fix py3 print usages and trailing whitespaces (`#469 `_) -* Drop separate node for message service (`#446 `_) -* Fix reconnection of rosserial-python (`#445 `_) -* Contributors: Asuki Kono, Daisuke Sato, Hermann von Kleist, Hikaru Sugiura, Mike Purvis, acxz - -0.8.0 (2018-10-11) ------------------- -* fix no attribute message_cache issue in message_info_service (`#393 `_) -* Added service to force an Arduino hard reset in serial_node.py (`#349 `_) - * Added hard_reset service call to serial_node - * Refactored SerialClient to use a write thread, working around deadlock when both Arduino and serial_node.py get stuck writing to each other. - * Updated cmakelists and package.xml to include dependencies. Removed unnecessary tcp functionality from arduino-specific serial_node.py -* Add support for boolean parameters (`#355 `_) -* [python] fix an unboundlocalerror (`#346 `_) -* Retry opening the serial port every 3 seconds (`#342 `_) - * Retry opening the serial port every 3 seconds - * Break out of the retry loop if we've been shut down -* Contributors: Chris Spencer, Kenta Yonekura, Pikrass, dlguo-cpr - -0.7.7 (2017-11-29) ------------------- -* Fix catkin lint errors (`#296 `_) -* pyserial bug workaround to fix rosserial_test test against SerialClient.py (`#313 `_) -* Add requestTopics to correct publish before setup (`#308 `_) -* Contributors: Bei Chen Liu, Tom O'Connell - -0.7.6 (2017-03-01) ------------------- -* Fix typo in serial error message (`#253 `_) -* Contributors: Jonathan Binney - -0.7.5 (2016-11-22) ------------------- - -0.7.4 (2016-09-21) ------------------- -* Try to read more serial bytes in a loop (`#248 `_) -* Add missing "import errno" to rosserial_python -* Integration tests for rosserial (`#243 `_) -* rosserial_python tcp server allowing socket address reuse (`#242 `_) -* Contributors: Mike Purvis, Vitor Matos, davidshin172 - -0.7.3 (2016-08-05) ------------------- - -0.7.2 (2016-07-15) ------------------- - -0.7.1 (2015-07-06) ------------------- - -0.7.0 (2015-04-23) ------------------- -* Adds default queue_size of 10 for rosserial_python publisher. -* Fixed queue size warning with diagnostics publisher. -* We don't need roslib.load_manifest any more under catkin. -* Contributors: Basheer Subei, David Lavoie-Boutin, Mike Purvis, eisoku9618 - -0.6.3 (2014-11-05) ------------------- - -0.6.2 (2014-09-10) ------------------- -* Added MD5 verification for request and response messags upon ServiceClient registration. -* Enabled registration of service clients -* Contributors: Jonathan Jekir - -0.6.1 (2014-06-30) ------------------- - -0.6.0 (2014-06-11) ------------------- - -0.5.6 (2014-06-11) ------------------- -* Add Mike Purvis as maintainer to all but xbee. -* Added the missing inWaiting() to RosSerialServer -* improvement: inform user of mismatched checksum for topic_id and msg -* Fix indent on if-block. -* Check for data in buffer before attempting to tryRead. Insert a 1ms sleep to avoid pegging the processor. -* Better warning message for tryRead. -* fix two points: 1. the number of bytes to read for chk_byte, 2. the wrong indentation about the defination of sendDiagnostics() -* Try-block to handle IOErrors thrown from tryRead -* Merge from hydro-devel. -* fix the dupilcated registration problem of subscriber -* remove ID_TX_STOP from rosserial_msgs/msg/TopicInfo.msg, using hardcode modification. fix the dupilcated registration problem of subscriber -* modified rosserial -* modified rosserial -* Contributors: Girts Linde, Mike Purvis, Moju Zhao, bakui, denis - -0.5.5 (2014-01-14) ------------------- - -0.5.4 (2013-10-17) ------------------- - -0.5.3 (2013-09-21) ------------------- -* De-register subscribers and service clients upon disconnect. - This prevents callbacks being called after a client program - terminates a connection. -* Fill out package.xml properly, include docstring in helper Python node. -* Add message info helper script that supports rosserial_server - -0.5.2 (2013-07-17) ------------------- - -* Fix release version - -0.5.1 (2013-07-15) ------------------- -* Merge branch 'rosserial_bakui' of git://github.com/tongtybj/rosserial into tongtybj-rosserial_bakui - * Modified the frame structure for serial communication, particularly add the checksum for msg_len -* Incorporate protocol version in message. Try to detect protocol version mismatch and give appropriate hints. - -0.4.5 (2013-07-02) ------------------- -* Fix SeviceServer member names in error message - 'm' prefix was omitted, causing an exception while trying to print - an error about md5 mismatches. Fix this to allow the error to be - presented to the user. -* Allow service calls with empty requests - std_srvs::Empty has a request message of size zero. SerialClient.send - returns the size of the sent message, which is checked to ensure - data crossed the serial line. Accommodate services with empty requests - by modifying the check to acknowledge all transmissions of zero or - more bytes as valid. -* revert name of node, add a few comments/spacing -* fix private parameters - temporary fix breaks fork_server for tcp -* Fix `#35 `_ - -0.4.4 (2013-03-20) ------------------- -* Fixed "Lost sync" message at initial connection that happens on both Arduino & - embeddedLinux. Problem was last_sync initialized to epoch and compared against - Time.now() always times out on first compare. - -0.4.3 (2013-03-13 14:08) ------------------------- - -0.4.2 (2013-03-13 01:15) ------------------------- - -0.4.1 (2013-03-09) ------------------- - -0.4.0 (2013-03-08) ------------------- -* initial catkin version on github diff --git a/src/rosserial_python/CMakeLists.txt b/src/rosserial_python/CMakeLists.txt deleted file mode 100644 index ce8cb35..0000000 --- a/src/rosserial_python/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.7.2) -project(rosserial_python) - -find_package(catkin REQUIRED) -catkin_package() - -catkin_python_setup() - -catkin_install_python( - PROGRAMS nodes/message_info_service.py nodes/serial_node.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/src/rosserial_python/nodes/message_info_service.py b/src/rosserial_python/nodes/message_info_service.py deleted file mode 100644 index 2d21148..0000000 --- a/src/rosserial_python/nodes/message_info_service.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python - -##################################################################### -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Clearpath Robotics -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" This functionality is no longer needed, as it's now handled - by accessing the Python interpreter directly from rosserial_server. - The file remains to avoid breaking launch files which include it.""" - -import rospy - - -if __name__=="__main__": - rospy.init_node('message_info_service') - rospy.logwarn("The message info service node is no longer needed and will be " - "removed in a future release of rosserial.") - rospy.spin() diff --git a/src/rosserial_python/nodes/serial_node.py b/src/rosserial_python/nodes/serial_node.py deleted file mode 100644 index ddbcf83..0000000 --- a/src/rosserial_python/nodes/serial_node.py +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env python - -##################################################################### -# Software License Agreement (BSD License) -# -# Copyright (c) 2011, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -__author__ = "mferguson@willowgarage.com (Michael Ferguson)" - -import rospy -from rosserial_python import SerialClient, RosSerialServer -from serial import SerialException -from time import sleep -import multiprocessing - -import sys - -if __name__=="__main__": - rospy.init_node("serial_node") - rospy.loginfo("ROS Serial Python Node") - - port_name = rospy.get_param('~port','/dev/ttyUSB0') - baud = int(rospy.get_param('~baud','57600')) - - # for systems where pyserial yields errors in the fcntl.ioctl(self.fd, TIOCMBIS, \ - # TIOCM_DTR_str) line, which causes an IOError, when using simulated port - fix_pyserial_for_test = rospy.get_param('~fix_pyserial_for_test', False) - - # Allows for assigning local parameters for tcp_port and fork_server with - # global parameters as fallback to prevent breaking changes - if(rospy.has_param('~tcp_port')): - tcp_portnum = int(rospy.get_param('~tcp_port')) - else: - tcp_portnum = int(rospy.get_param('/rosserial_embeddedlinux/tcp_port', '11411')) - - if(rospy.has_param('~fork_server')): - fork_server = rospy.get_param('~fork_server') - else: - fork_server = rospy.get_param('/rosserial_embeddedlinux/fork_server', False) - - # TODO: do we really want command line params in addition to parameter server params? - sys.argv = rospy.myargv(argv=sys.argv) - if len(sys.argv) >= 2 : - port_name = sys.argv[1] - if len(sys.argv) == 3 : - tcp_portnum = int(sys.argv[2]) - - if port_name == "tcp" : - server = RosSerialServer(tcp_portnum, fork_server) - rospy.loginfo("Waiting for socket connections on port %d" % tcp_portnum) - try: - server.listen() - except KeyboardInterrupt: - rospy.loginfo("got keyboard interrupt") - finally: - rospy.loginfo("Shutting down") - for process in multiprocessing.active_children(): - rospy.loginfo("Shutting down process %r", process) - process.terminate() - process.join() - rospy.loginfo("All done") - - else : # Use serial port - while not rospy.is_shutdown(): - rospy.loginfo("Connecting to %s at %d baud" % (port_name,baud) ) - try: - client = SerialClient(port_name, baud, fix_pyserial_for_test=fix_pyserial_for_test) - client.run() - except KeyboardInterrupt: - break - except SerialException: - sleep(1.0) - continue - except OSError: - sleep(1.0) - continue - except: - rospy.logwarn("Unexpected Error: %s", sys.exc_info()[0]) - client.port.close() - sleep(1.0) - continue diff --git a/src/rosserial_python/package.xml b/src/rosserial_python/package.xml deleted file mode 100644 index e5a4f98..0000000 --- a/src/rosserial_python/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - rosserial_python - 0.9.2 - - A Python-based implementation of the rosserial protocol. - - Michael Ferguson - Paul Bouchier - Mike Purvis - BSD - http://ros.org/wiki/rosserial_python - - catkin - - rospy - rosserial_msgs - diagnostic_msgs - python3-serial - diff --git a/src/rosserial_python/setup.py b/src/rosserial_python/setup.py deleted file mode 100644 index 1376939..0000000 --- a/src/rosserial_python/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rosserial_python'], - package_dir={'': 'src'}, - ) - -setup(**d) diff --git a/src/rosserial_python/src/rosserial_python/SerialClient.py b/src/rosserial_python/src/rosserial_python/SerialClient.py deleted file mode 100644 index 6b1b875..0000000 --- a/src/rosserial_python/src/rosserial_python/SerialClient.py +++ /dev/null @@ -1,821 +0,0 @@ -##################################################################### -# Software License Agreement (BSD License) -# -# Copyright (c) 2011, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -__author__ = "mferguson@willowgarage.com (Michael Ferguson)" - -import array -import errno -import imp -import io -import multiprocessing -import queue -import socket -import struct -import sys -import threading -import time - -from serial import Serial, SerialException, SerialTimeoutException - -import roslib -import rospy -from std_msgs.msg import Time -from rosserial_msgs.msg import TopicInfo, Log -from rosserial_msgs.srv import RequestParamRequest, RequestParamResponse - -import diagnostic_msgs.msg - -ERROR_MISMATCHED_PROTOCOL = "Mismatched protocol version in packet: lost sync or rosserial_python is from different ros release than the rosserial client" -ERROR_NO_SYNC = "no sync with device" -ERROR_PACKET_FAILED = "Packet Failed : Failed to read msg data" - -def load_pkg_module(package, directory): - #check if its in the python path - path = sys.path - try: - imp.find_module(package) - except ImportError: - roslib.load_manifest(package) - try: - m = __import__( package + '.' + directory ) - except ImportError: - rospy.logerr( "Cannot import package : %s"% package ) - rospy.logerr( "sys.path was " + str(path) ) - return None - return m - -def load_message(package, message): - m = load_pkg_module(package, 'msg') - m2 = getattr(m, 'msg') - return getattr(m2, message) - -def load_service(package,service): - s = load_pkg_module(package, 'srv') - s = getattr(s, 'srv') - srv = getattr(s, service) - mreq = getattr(s, service+"Request") - mres = getattr(s, service+"Response") - return srv,mreq,mres - -class Publisher: - """ - Publisher forwards messages from the serial device to ROS. - """ - def __init__(self, topic_info): - """ Create a new publisher. """ - self.topic = topic_info.topic_name - - # find message type - package, message = topic_info.message_type.split('/') - self.message = load_message(package, message) - if self.message._md5sum == topic_info.md5sum: - self.publisher = rospy.Publisher(self.topic, self.message, queue_size=10) - else: - raise Exception('Checksum does not match: ' + self.message._md5sum + ',' + topic_info.md5sum) - - def handlePacket(self, data): - """ Forward message to ROS network. """ - m = self.message() - m.deserialize(data) - self.publisher.publish(m) - - -class Subscriber: - """ - Subscriber forwards messages from ROS to the serial device. - """ - - def __init__(self, topic_info, parent): - self.topic = topic_info.topic_name - self.id = topic_info.topic_id - self.parent = parent - - # find message type - package, message = topic_info.message_type.split('/') - self.message = load_message(package, message) - if self.message._md5sum == topic_info.md5sum: - self.subscriber = rospy.Subscriber(self.topic, self.message, self.callback) - else: - raise Exception('Checksum does not match: ' + self.message._md5sum + ',' + topic_info.md5sum) - - def callback(self, msg): - """ Forward message to serial device. """ - data_buffer = io.BytesIO() - msg.serialize(data_buffer) - self.parent.send(self.id, data_buffer.getvalue()) - - def unregister(self): - rospy.loginfo("Removing subscriber: %s", self.topic) - self.subscriber.unregister() - -class ServiceServer: - """ - ServiceServer responds to requests from ROS. - """ - - def __init__(self, topic_info, parent): - self.topic = topic_info.topic_name - self.parent = parent - - # find message type - package, service = topic_info.message_type.split('/') - s = load_pkg_module(package, 'srv') - s = getattr(s, 'srv') - self.mreq = getattr(s, service+"Request") - self.mres = getattr(s, service+"Response") - srv = getattr(s, service) - self.service = rospy.Service(self.topic, srv, self.callback) - - # response message - self.data = None - - def unregister(self): - rospy.loginfo("Removing service: %s", self.topic) - self.service.shutdown() - - def callback(self, req): - """ Forward request to serial device. """ - data_buffer = io.BytesIO() - req.serialize(data_buffer) - self.response = None - self.parent.send(self.id, data_buffer.getvalue()) - while self.response is None: - pass - return self.response - - def handlePacket(self, data): - """ Forward response to ROS network. """ - r = self.mres() - r.deserialize(data) - self.response = r - - -class ServiceClient: - """ - ServiceServer responds to requests from ROS. - """ - - def __init__(self, topic_info, parent): - self.topic = topic_info.topic_name - self.parent = parent - - # find message type - package, service = topic_info.message_type.split('/') - s = load_pkg_module(package, 'srv') - s = getattr(s, 'srv') - self.mreq = getattr(s, service+"Request") - self.mres = getattr(s, service+"Response") - srv = getattr(s, service) - rospy.loginfo("Starting service client, waiting for service '" + self.topic + "'") - rospy.wait_for_service(self.topic) - self.proxy = rospy.ServiceProxy(self.topic, srv) - - def handlePacket(self, data): - """ Forward request to ROS network. """ - req = self.mreq() - req.deserialize(data) - # call service proxy - resp = self.proxy(req) - # serialize and publish - data_buffer = io.BytesIO() - resp.serialize(data_buffer) - self.parent.send(self.id, data_buffer.getvalue()) - -class RosSerialServer: - """ - RosSerialServer waits for a socket connection then passes itself, forked as a - new process, to SerialClient which uses it as a serial port. It continues to listen - for additional connections. Each forked process is a new ros node, and proxies ros - operations (e.g. publish/subscribe) from its connection to the rest of ros. - """ - def __init__(self, tcp_portnum, fork_server=False): - rospy.loginfo("Fork_server is: %s" % fork_server) - self.tcp_portnum = tcp_portnum - self.fork_server = fork_server - - def listen(self): - self.serversocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.serversocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - #bind the socket to a public host, and a well-known port - self.serversocket.bind(("", self.tcp_portnum)) #become a server socket - self.serversocket.listen(1) - self.serversocket.settimeout(1) - - #accept connections - rospy.loginfo("Waiting for socket connection") - while not rospy.is_shutdown(): - try: - clientsocket, address = self.serversocket.accept() - except socket.timeout: - continue - - #now do something with the clientsocket - rospy.loginfo("Established a socket connection from %s on port %s" % address) - self.socket = clientsocket - self.isConnected = True - - if self.fork_server: # if configured to launch server in a separate process - rospy.loginfo("Forking a socket server process") - process = multiprocessing.Process(target=self.startSocketServer, args=address) - process.daemon = True - process.start() - rospy.loginfo("launched startSocketServer") - else: - rospy.loginfo("calling startSerialClient") - self.startSerialClient() - rospy.loginfo("startSerialClient() exited") - - def startSerialClient(self): - client = SerialClient(self) - try: - client.run() - except KeyboardInterrupt: - pass - except RuntimeError: - rospy.loginfo("RuntimeError exception caught") - self.isConnected = False - except socket.error: - rospy.loginfo("socket.error exception caught") - self.isConnected = False - finally: - rospy.loginfo("Client has exited, closing socket.") - self.socket.close() - for sub in client.subscribers.values(): - sub.unregister() - for srv in client.services.values(): - srv.unregister() - - def startSocketServer(self, port, address): - rospy.loginfo("starting ROS Serial Python Node serial_node-%r" % address) - rospy.init_node("serial_node_%r" % address) - self.startSerialClient() - - def flushInput(self): - pass - - def write(self, data): - if not self.isConnected: - return - length = len(data) - totalsent = 0 - - while totalsent < length: - try: - totalsent += self.socket.send(data[totalsent:]) - except BrokenPipeError: - raise RuntimeError("RosSerialServer.write() socket connection broken") - - def read(self, rqsted_length): - self.msg = b'' - if not self.isConnected: - return self.msg - - while len(self.msg) < rqsted_length: - chunk = self.socket.recv(rqsted_length - len(self.msg)) - if chunk == b'': - raise RuntimeError("RosSerialServer.read() socket connection broken") - self.msg = self.msg + chunk - return self.msg - - def inWaiting(self): - try: # the caller checks just for <1, so we'll peek at just one byte - chunk = self.socket.recv(1, socket.MSG_DONTWAIT|socket.MSG_PEEK) - if chunk == b'': - raise RuntimeError("RosSerialServer.inWaiting() socket connection broken") - return len(chunk) - except BlockingIOError: - return 0 - -class SerialClient(object): - """ - ServiceServer responds to requests from the serial device. - """ - header = b'\xff' - - # hydro introduces protocol ver2 which must match node_handle.h - # The protocol version is sent as the 2nd sync byte emitted by each end - protocol_ver1 = b'\xff' - protocol_ver2 = b'\xfe' - protocol_ver = protocol_ver2 - - def __init__(self, port=None, baud=57600, timeout=5.0, fix_pyserial_for_test=False): - """ Initialize node, connect to bus, attempt to negotiate topics. """ - - self.read_lock = threading.RLock() - - self.write_lock = threading.RLock() - self.write_queue = queue.Queue() - self.write_thread = None - - self.lastsync = rospy.Time(0) - self.lastsync_lost = rospy.Time(0) - self.lastsync_success = rospy.Time(0) - self.last_read = rospy.Time(0) - self.last_write = rospy.Time(0) - self.timeout = timeout - self.synced = False - self.fix_pyserial_for_test = fix_pyserial_for_test - - self.publishers = dict() # id:Publishers - self.subscribers = dict() # topic:Subscriber - self.services = dict() # topic:Service - - def shutdown(): - self.txStopRequest() - rospy.loginfo('shutdown hook activated') - rospy.on_shutdown(shutdown) - - self.pub_diagnostics = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray, queue_size=10) - - if port is None: - # no port specified, listen for any new port? - pass - elif hasattr(port, 'read'): - #assume its a filelike object - self.port=port - else: - # open a specific port - while not rospy.is_shutdown(): - try: - if self.fix_pyserial_for_test: - # see https://github.com/pyserial/pyserial/issues/59 - self.port = Serial(port, baud, timeout=self.timeout, write_timeout=10, rtscts=True, dsrdtr=True) - else: - self.port = Serial(port, baud, timeout=self.timeout, write_timeout=10) - break - except SerialException as e: - rospy.logerr("Error opening serial: %s", e) - time.sleep(3) - - if rospy.is_shutdown(): - return - - time.sleep(0.1) # Wait for ready (patch for Uno) - - self.buffer_out = -1 - self.buffer_in = -1 - - self.callbacks = dict() - # endpoints for creating new pubs/subs - self.callbacks[TopicInfo.ID_PUBLISHER] = self.setupPublisher - self.callbacks[TopicInfo.ID_SUBSCRIBER] = self.setupSubscriber - # service client/servers have 2 creation endpoints (a publisher and a subscriber) - self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_PUBLISHER] = self.setupServiceServerPublisher - self.callbacks[TopicInfo.ID_SERVICE_SERVER+TopicInfo.ID_SUBSCRIBER] = self.setupServiceServerSubscriber - self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_PUBLISHER] = self.setupServiceClientPublisher - self.callbacks[TopicInfo.ID_SERVICE_CLIENT+TopicInfo.ID_SUBSCRIBER] = self.setupServiceClientSubscriber - # custom endpoints - self.callbacks[TopicInfo.ID_PARAMETER_REQUEST] = self.handleParameterRequest - self.callbacks[TopicInfo.ID_LOG] = self.handleLoggingRequest - self.callbacks[TopicInfo.ID_TIME] = self.handleTimeRequest - - rospy.sleep(2.0) - self.requestTopics() - self.lastsync = rospy.Time.now() - - def requestTopics(self): - """ Determine topics to subscribe/publish. """ - rospy.loginfo('Requesting topics...') - - # TODO remove if possible - if not self.fix_pyserial_for_test: - with self.read_lock: - self.port.flushInput() - - # request topic sync - self.write_queue.put(self.header + self.protocol_ver + b"\x00\x00\xff\x00\x00\xff") - - def txStopRequest(self): - """ Send stop tx request to client before the node exits. """ - if not self.fix_pyserial_for_test: - with self.read_lock: - self.port.flushInput() - - self.write_queue.put(self.header + self.protocol_ver + b"\x00\x00\xff\x0b\x00\xf4") - rospy.loginfo("Sending tx stop request") - - def tryRead(self, length): - try: - read_start = time.time() - bytes_remaining = length - result = bytearray() - while bytes_remaining != 0 and time.time() - read_start < self.timeout: - with self.read_lock: - received = self.port.read(bytes_remaining) - if len(received) != 0: - self.last_read = rospy.Time.now() - result.extend(received) - bytes_remaining -= len(received) - - if bytes_remaining != 0: - raise IOError("Returned short (expected %d bytes, received %d instead)." % (length, length - bytes_remaining)) - - return bytes(result) - except Exception as e: - raise IOError("Serial Port read failure: %s" % e) - - def run(self): - """ Forward recieved messages to appropriate publisher. """ - - # Launch write thread. - if self.write_thread is None: - self.write_thread = threading.Thread(target=self.processWriteQueue) - self.write_thread.daemon = True - self.write_thread.start() - - # Handle reading. - data = '' - read_step = None - while self.write_thread.is_alive() and not rospy.is_shutdown(): - if (rospy.Time.now() - self.lastsync).to_sec() > (self.timeout * 3): - if self.synced: - rospy.logerr("Lost sync with device, restarting...") - else: - rospy.logerr("Unable to sync with device; possible link problem or link software version mismatch such as hydro rosserial_python with groovy Arduino") - self.lastsync_lost = rospy.Time.now() - self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, ERROR_NO_SYNC) - self.requestTopics() - self.lastsync = rospy.Time.now() - - # This try-block is here because we make multiple calls to read(). Any one of them can throw - # an IOError if there's a serial problem or timeout. In that scenario, a single handler at the - # bottom attempts to reconfigure the topics. - try: - with self.read_lock: - if self.port.inWaiting() < 1: - time.sleep(0.001) - continue - - # Find sync flag. - flag = [0, 0] - read_step = 'syncflag' - flag[0] = self.tryRead(1) - if (flag[0] != self.header): - continue - - # Find protocol version. - read_step = 'protocol' - flag[1] = self.tryRead(1) - if flag[1] != self.protocol_ver: - self.sendDiagnostics(diagnostic_msgs.msg.DiagnosticStatus.ERROR, ERROR_MISMATCHED_PROTOCOL) - rospy.logerr("Mismatched protocol version in packet (%s): lost sync or rosserial_python is from different ros release than the rosserial client" % repr(flag[1])) - protocol_ver_msgs = { - self.protocol_ver1: 'Rev 0 (rosserial 0.4 and earlier)', - self.protocol_ver2: 'Rev 1 (rosserial 0.5+)', - b'\xfd': 'Some future rosserial version' - } - if flag[1] in protocol_ver_msgs: - found_ver_msg = 'Protocol version of client is ' + protocol_ver_msgs[flag[1]] - else: - found_ver_msg = "Protocol version of client is unrecognized" - rospy.loginfo("%s, expected %s" % (found_ver_msg, protocol_ver_msgs[self.protocol_ver])) - continue - - # Read message length, checksum (3 bytes) - read_step = 'message length' - msg_len_bytes = self.tryRead(3) - msg_length, _ = struct.unpack(" 0 and length > self.buffer_in: - rospy.logerr("Message from ROS network dropped: message larger than buffer.\n%s" % msg) - return -1 - else: - # frame : header (1b) + version (1b) + msg_len(2b) + msg_len_chk(1b) + topic_id(2b) + msg(nb) + msg_topic_id_chk(1b) - length_bytes = struct.pack('0: - status.values[0].value=time.ctime(self.lastsync.to_sec()) - else: - status.values[0].value="never" - - status.values.append(diagnostic_msgs.msg.KeyValue()) - status.values[1].key="last sync lost" - status.values[1].value=time.ctime(self.lastsync_lost.to_sec()) - - self.pub_diagnostics.publish(msg) diff --git a/src/rosserial_python/src/rosserial_python/__init__.py b/src/rosserial_python/src/rosserial_python/__init__.py deleted file mode 100644 index e4f3ed8..0000000 --- a/src/rosserial_python/src/rosserial_python/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from .SerialClient import * diff --git a/src/rosserial_python/src/rosserial_python/__pycache__/SerialClient.cpython-38.pyc b/src/rosserial_python/src/rosserial_python/__pycache__/SerialClient.cpython-38.pyc deleted file mode 100644 index 0088f81..0000000 Binary files a/src/rosserial_python/src/rosserial_python/__pycache__/SerialClient.cpython-38.pyc and /dev/null differ diff --git a/src/service/CMakeLists.txt b/src/service/CMakeLists.txt deleted file mode 100755 index bf7dd5f..0000000 --- a/src/service/CMakeLists.txt +++ /dev/null @@ -1,209 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(service) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - geometry_msgs - turtlesim -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES service -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/service.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(day6_practice src/day6_practice.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(day6_practice - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - Practice-Launch.launch - # myfile1 - # myfile2 - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_service.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/service/config/Practice-Param.yaml b/src/service/config/Practice-Param.yaml deleted file mode 100644 index 9c179ef..0000000 --- a/src/service/config/Practice-Param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -footprint_x: 1 -footprint_y: 1 -linear_velocity: 0.1 -angular_velocity: 0.1 \ No newline at end of file diff --git a/src/service/launch/Practice-Launch.launch b/src/service/launch/Practice-Launch.launch deleted file mode 100644 index 5315a72..0000000 --- a/src/service/launch/Practice-Launch.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/src/service/package.xml b/src/service/package.xml deleted file mode 100755 index e0c5f6e..0000000 --- a/src/service/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - service - 0.0.0 - The service package - - - - - user - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/src/service/src/day6_practice.cpp b/src/service/src/day6_practice.cpp deleted file mode 100644 index b442de5..0000000 --- a/src/service/src/day6_practice.cpp +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include -#include -#include - - -/*Global Variable*/ -turtlesim::Pose CurrentPose; -turtlesim::Pose TargetPose; -double footprint_x = 0; -double footprint_y = 0; -geometry_msgs::Twist current_vel; -double linear_vel = 0.0; -double angular_vel = 0.0; -double target_theta = 0; -double m = 0; -bool get = 0; -/*Global Variable*/ - -/*function definition*/ -void Callback(const turtlesim::Pose::ConstPtr& pose); -/*function definition*/ - - -int main(int argc, char *argv[]) -{ - ros::init(argc, argv, "practice_main"); - ros::NodeHandle nh; - ros::Publisher turtle_vel_pub = nh.advertise("/turtle1/cmd_vel", 100); - ros::Subscriber turtle_pose_sub = nh.subscribe("/turtle1/pose", 100, Callback); - ros::Rate loop_rate(1); - - if(nh.getParam("/linear_velocity", linear_vel) && nh.getParam("/angular_velocity", angular_vel)){ - - }else{ - ROS_ERROR("FAIL_TO_GET_PARAM(VEL)"); - return 0; - } - - - if(nh.getParam("footprint_x", footprint_x) && nh.getParam("footprint_y", footprint_y)){ - TargetPose.x = footprint_x; - TargetPose.y = footprint_y; - }else{ - ROS_ERROR("FAIL_TO_GET_PARAM(FOOTPRINT)"); - return 0; - } - - - - while (ros::ok()) - { - current_vel.angular.z = 0; - if(abs(CurrentPose.x-footprint_x)>=0.15){ - if(CurrentPose.x-footprint_x > 0){ - current_vel.linear.x = -abs(linear_vel); - }else{ - current_vel.linear.x = abs(linear_vel); - } - }else{ - current_vel.linear.x = 0; - } - if(abs(CurrentPose.y-footprint_y)>=0.15){ - if(CurrentPose.y-footprint_x > 0){ - current_vel.linear.y = -abs(linear_vel); - }else{ - current_vel.linear.y = abs(linear_vel); - } - }else{ - current_vel.linear.y = 0; - } - - turtle_vel_pub.publish(current_vel); - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} - -void Callback(const turtlesim::Pose::ConstPtr& pose){ - CurrentPose.x = pose->x; - CurrentPose.y = pose->y; - CurrentPose.theta = pose->theta; - ROS_ERROR("%f", CurrentPose.x); - ROS_ERROR("%f", CurrentPose.y); - ROS_ERROR("%f", CurrentPose.theta); -} \ No newline at end of file diff --git a/src/tf/CMakeLists.txt b/src/tf/CMakeLists.txt deleted file mode 100755 index dbad98e..0000000 --- a/src/tf/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(tf) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES tf -# CATKIN_DEPENDS roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/tf.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/tf_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_tf.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/src/tf/package.xml b/src/tf/package.xml deleted file mode 100755 index 820f8ff..0000000 --- a/src/tf/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - tf - 0.0.0 - The tf package - - - - - user - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - -