diff --git a/autonomy/teleop/README.md b/autonomy/teleop/README.md new file mode 100644 index 0000000..1a52eb0 --- /dev/null +++ b/autonomy/teleop/README.md @@ -0,0 +1,45 @@ +# Teleop +## Samples +To run rosbridge listener: +``` +ros2 run rosbridge_listener rosbridge_listener_node +``` + +To run rosbridge publisher: + +``` +ros2 run rosbridge_publisher rosbridge_publisher_node +``` + +## Running Rosbridge Server +Rosbridge server will relay ros topics to humanoid-server. To run, use: +``` +ros2 launch rosbridge_server rosbridge_websocket_launch.xml +``` + +## Full Setup for Testing VR -> ROS +These commands were run with the autonomy/samples folder deleted, since some packages may overlap + +```bash + +# In humanoid repo: +cd autonomy +colcon build +source install/setup.bash +ros2 run rosbridge_listener rosbridge_listener_node + +# Then, in another terminal: +cd autonomy +source install/setup.bash +ros2 launch rosbridge_server rosbridge_websocket_launch.xml + +# In humanoid-server repo: +npm run dev +``` + +Then, connect to ws://localhost:3000 (default websocket url) and send an example hand pose message: +``` +[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0] +``` +The message should appear in the terminal where you ran the rosbridge listener node + diff --git a/autonomy/teleop/rosbridge_listener/CMakeLists.txt b/autonomy/teleop/rosbridge_listener/CMakeLists.txt new file mode 100644 index 0000000..1a56c21 --- /dev/null +++ b/autonomy/teleop/rosbridge_listener/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(rosbridge_listener) + +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(sample_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_library(rosbridge_listener_lib + src/rosbridge_listener_core.cpp) + +target_include_directories(rosbridge_listener_lib + PUBLIC include) + +add_executable(rosbridge_listener_node src/rosbridge_listener_node.cpp) + +ament_target_dependencies(rosbridge_listener_node rclcpp std_msgs sample_msgs) +target_include_directories(rosbridge_listener_node PUBLIC + $ + $) +target_compile_features(rosbridge_listener_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS rosbridge_listener_node + 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/autonomy/teleop/rosbridge_listener/include/rosbridge_listener_core.hpp b/autonomy/teleop/rosbridge_listener/include/rosbridge_listener_core.hpp new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/teleop/rosbridge_listener/include/rosbridge_listener_node.hpp b/autonomy/teleop/rosbridge_listener/include/rosbridge_listener_node.hpp new file mode 100644 index 0000000..5808f83 --- /dev/null +++ b/autonomy/teleop/rosbridge_listener/include/rosbridge_listener_node.hpp @@ -0,0 +1,27 @@ +#ifndef ROSBRIDGE_LISTENER_NODE_HPP_ +#define ROSBRIDGE_LISTENER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/hand_pose.hpp" + +#include "rosbridge_listener_core.hpp" + +/** + * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered" + * topics and echoes the operating frequency of the topic to the console. + */ +class ROSbridgeListenerNode : public rclcpp::Node { +public: + ROSbridgeListenerNode(); + +private: + std::string + hand_pose_to_string(const sample_msgs::msg::HandPose::SharedPtr pose); + + void hand_pose_callback(const sample_msgs::msg::HandPose::SharedPtr msg); + + rclcpp::Subscription::SharedPtr hand_pose_sub_; +}; + +#endif \ No newline at end of file diff --git a/autonomy/teleop/rosbridge_listener/package.xml b/autonomy/teleop/rosbridge_listener/package.xml new file mode 100644 index 0000000..e70774a --- /dev/null +++ b/autonomy/teleop/rosbridge_listener/package.xml @@ -0,0 +1,21 @@ + + + + rosbridge_listener + 0.0.0 + TODO: Package description + yichen + TODO: License declaration + + ament_cmake + + rclcpp + sample_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/autonomy/teleop/rosbridge_listener/src/rosbridge_listener_core.cpp b/autonomy/teleop/rosbridge_listener/src/rosbridge_listener_core.cpp new file mode 100644 index 0000000..e69de29 diff --git a/autonomy/teleop/rosbridge_listener/src/rosbridge_listener_node.cpp b/autonomy/teleop/rosbridge_listener/src/rosbridge_listener_node.cpp new file mode 100644 index 0000000..7c2be64 --- /dev/null +++ b/autonomy/teleop/rosbridge_listener/src/rosbridge_listener_node.cpp @@ -0,0 +1,38 @@ +#include "rosbridge_listener_node.hpp" + +#include +#include + +ROSbridgeListenerNode::ROSbridgeListenerNode() : Node("rosbridge_listener") { + + hand_pose_sub_ = this->create_subscription( + "teleop", 10, + std::bind(&ROSbridgeListenerNode::hand_pose_callback, this, + std::placeholders::_1)); +} + +std::string ROSbridgeListenerNode::hand_pose_to_string( + const sample_msgs::msg::HandPose::SharedPtr pose) { + std::string s; + std::vector hand_pose = pose->data; + s += "["; + for (int i = 0; i < hand_pose.size(); i++) { + s += std::to_string(hand_pose[i]); + s += ","; + } + s += "]"; + return s; +} + +void ROSbridgeListenerNode::hand_pose_callback( + const sample_msgs::msg::HandPose::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Received Hand Data: %s", + hand_pose_to_string(msg).c_str()); +} + +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/autonomy/teleop/rosbridge_publisher/CMakeLists.txt b/autonomy/teleop/rosbridge_publisher/CMakeLists.txt new file mode 100644 index 0000000..61a2d10 --- /dev/null +++ b/autonomy/teleop/rosbridge_publisher/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(rosbridge_publisher) + +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(std_msgs REQUIRED) +find_package(sample_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_library(rosbridge_publisher_lib src/rosbridge_publisher_core.cpp) + +target_include_directories(rosbridge_publisher_lib PUBLIC include) + +add_executable(rosbridge_publisher_node src/rosbridge_publisher_node.cpp) +ament_target_dependencies(rosbridge_publisher_lib rclcpp std_msgs sample_msgs) +target_link_libraries(rosbridge_publisher_node rosbridge_publisher_lib) +target_include_directories(rosbridge_publisher_node PUBLIC + $ + $) +target_compile_features(rosbridge_publisher_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS rosbridge_publisher_node + 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/autonomy/teleop/rosbridge_publisher/include/rosbridge_publisher_core.hpp b/autonomy/teleop/rosbridge_publisher/include/rosbridge_publisher_core.hpp new file mode 100644 index 0000000..bde748f --- /dev/null +++ b/autonomy/teleop/rosbridge_publisher/include/rosbridge_publisher_core.hpp @@ -0,0 +1,17 @@ +#ifndef ROSBRIDGE_PUBLISHER_CORE_HPP_ +#define ROSBRIDGE_PUBLISHER_CORE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/hand_pose.hpp" + +class ROSbridgePublisherCore { +public: + explicit ROSbridgePublisherCore(); + + std::string hand_pose_to_string(const sample_msgs::msg::HandPose pose); + +private: +}; + +#endif // AGGREGATOR_CORE_HPP_ diff --git a/autonomy/teleop/rosbridge_publisher/include/rosbridge_publisher_node.hpp b/autonomy/teleop/rosbridge_publisher/include/rosbridge_publisher_node.hpp new file mode 100644 index 0000000..e2d85ab --- /dev/null +++ b/autonomy/teleop/rosbridge_publisher/include/rosbridge_publisher_node.hpp @@ -0,0 +1,28 @@ +#ifndef ROSBRIDGE_PUBLISHER_NODE_HPP_ +#define ROSBRIDGE_PUBLISHER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "sample_msgs/msg/hand_pose.hpp" + +#include "rosbridge_publisher_core.hpp" + +/** + * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered" + * topics and echoes the operating frequency of the topic to the console. + */ +class ROSbridgePublisherNode : public rclcpp::Node { +public: + ROSbridgePublisherNode(); + +private: + void timer_callback(); + + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr hand_pose_pub_; + + ROSbridgePublisherCore rosbridge_publisher_; +}; + +#endif \ No newline at end of file diff --git a/autonomy/teleop/rosbridge_publisher/package.xml b/autonomy/teleop/rosbridge_publisher/package.xml new file mode 100644 index 0000000..1137e05 --- /dev/null +++ b/autonomy/teleop/rosbridge_publisher/package.xml @@ -0,0 +1,22 @@ + + + + rosbridge_publisher + 0.0.0 + TODO: Package description + yichen + TODO: License declaration + + ament_cmake + + rclcpp + sample_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/autonomy/teleop/rosbridge_publisher/src/rosbridge_publisher_core.cpp b/autonomy/teleop/rosbridge_publisher/src/rosbridge_publisher_core.cpp new file mode 100644 index 0000000..649e6a6 --- /dev/null +++ b/autonomy/teleop/rosbridge_publisher/src/rosbridge_publisher_core.cpp @@ -0,0 +1,19 @@ +#include +#include + +#include "rosbridge_publisher_core.hpp" + +ROSbridgePublisherCore::ROSbridgePublisherCore() {} + +std::string ROSbridgePublisherCore::hand_pose_to_string( + const sample_msgs::msg::HandPose pose) { + std::string s; + std::vector hand_pose = pose.data; + s += "["; + for (int i = 0; i < hand_pose.size(); i++) { + s += std::to_string(hand_pose[i]); + s += ","; + } + s += "]"; + return s; +} \ No newline at end of file diff --git a/autonomy/teleop/rosbridge_publisher/src/rosbridge_publisher_node.cpp b/autonomy/teleop/rosbridge_publisher/src/rosbridge_publisher_node.cpp new file mode 100644 index 0000000..0fe923a --- /dev/null +++ b/autonomy/teleop/rosbridge_publisher/src/rosbridge_publisher_node.cpp @@ -0,0 +1,31 @@ +#include "rosbridge_publisher_node.hpp" + +#include +#include + +using namespace std::chrono_literals; + +ROSbridgePublisherNode::ROSbridgePublisherNode() + : Node("rosbridge_publisher"), + rosbridge_publisher_(ROSbridgePublisherCore()) { + + hand_pose_pub_ = + this->create_publisher("teleop", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&ROSbridgePublisherNode::timer_callback, this)); +} + +void ROSbridgePublisherNode::timer_callback() { + auto message = sample_msgs::msg::HandPose(); + message.data = std::vector(17); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", + rosbridge_publisher_.hand_pose_to_string(message).c_str()); + hand_pose_pub_->publish(message); +} + +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/autonomy/wato_msgs/sample_msgs/CMakeLists.txt b/autonomy/wato_msgs/sample_msgs/CMakeLists.txt index 008fc0e..1bd6487 100644 --- a/autonomy/wato_msgs/sample_msgs/CMakeLists.txt +++ b/autonomy/wato_msgs/sample_msgs/CMakeLists.txt @@ -17,7 +17,8 @@ set(msg_files msg/Filtered.msg msg/FilteredArray.msg msg/Unfiltered.msg - msg/Metadata.msg) + msg/Metadata.msg + msg/HandPose.msg) rosidl_generate_interfaces(sample_msgs ${msg_files} DEPENDENCIES std_msgs) diff --git a/autonomy/wato_msgs/sample_msgs/msg/HandPose.msg b/autonomy/wato_msgs/sample_msgs/msg/HandPose.msg new file mode 100644 index 0000000..c5fd6c8 --- /dev/null +++ b/autonomy/wato_msgs/sample_msgs/msg/HandPose.msg @@ -0,0 +1 @@ +float32[] data \ No newline at end of file