updated srv def, src dir, readme

README.md

@@ -3,4 +3,8 @@
 
 1. run `./run.sh -b` to build the docker image, copy custom msgs into the docker image and compile ros1 bridge
 
-2. `./run.sh` to launch the container and interract with bridge, ros1 and ros2.
+2. `./run.sh -r` to launch the container and interract with bridge, ros1 and ros2.
+
+3. test the bridge
+
+source /home/nx/ros1_bridge_ws/install/setup.bash && source /home/nx/rosbridge_general_update_ament.sh '/home/nx/ros1_bridge_ws/install' 'ros1_bridge' && ros2 run ros1_bridge dynamic_bridge --print-pairs | grep custom
\ No newline at end of file

custom_bridge.Dockerfile

@@ -24,7 +24,7 @@ ARG PW=10
 
 
 ENV ROS2_DISTRO=humble
-ENV ROS2_INSTALL_PATH=$HOME/ros2_humble
+ENV ROS2_INSTALL_PATH=$HOME/ros2_ws
 RUN mkdir -p $ROS2_INSTALL_PATH/src
 ENV ROS2_INSTALL=$ROS2_INSTALL_PATH/install/setup.bash
 
@@ -125,14 +125,11 @@ RUN apt install -y \
 
 WORKDIR $HOME
 COPY ./rosbridge_general_update_ament.sh .
-RUN bash -c "./rosbridge_general_update_ament.sh"
-
-
 
 
 #### COPY CUSTOM_BRIDGE
 WORKDIR $HOME
-COPY ./custom_bridge_ros1_ws $HOME/custom_bridge_ros1_ws/src
+COPY ./custom_bridge_ros1_ws $HOME/custom_bridge_ros1_ws
 WORKDIR $HOME/custom_bridge_ros1_ws
 RUN bash -c "catkin_make"
 
@@ -142,7 +139,7 @@ RUN bash -c "catkin_make"
 #### COPY NEW BRIDGE WS
 COPY ./custom_bridge_ros2_ws $HOME/custom_bridge_ros2_ws
 WORKDIR $HOME/custom_bridge_ros2_ws
-RUN bash -c "source $HOME/ros2_humble/install/setup.bash && colcon build --symlink-install" 
+RUN bash -c "source $HOME/ros2_ws/install/setup.bash && colcon build --symlink-install" 
 
 
 RUN mkdir -p $HOME/.ros/log && \
@@ -159,7 +156,7 @@ RUN chown -R $USERNAME:$USERNAME $HOME/custom_bridge_ros2_ws && \
 
 
 #### INSTALL ROS1_BRIDGE
-ARG ROS1_BRIDGE_ROOT_DIR=/ros1_bridge_ws
+ARG ROS1_BRIDGE_ROOT_DIR=$HOME/ros1_bridge_ws
 ENV ROS1_BRIDGE_ROOT_DIR=$ROS1_BRIDGE_ROOT_DIR
 
 ARG ROS1_BRIDGE_INSTALL_DIR=$ROS1_BRIDGE_ROOT_DIR/src
@@ -175,10 +172,9 @@ WORKDIR $ROS1_BRIDGE_ROOT_DIR
 
 
 #### BUILD/COMPILE ROS1_BRIDGE
-# RUN bash -c "cd ${HOME}/ht_planner_ws \
-#     && source ./scripts/rosbridge_source.sh \
-#     && cd ${ROS1_BRIDGE_ROOT_DIR} \
-#     && colcon build --packages-select ros1_bridge --cmake-force-configure --parallel-workers $PW \
-#     "
+RUN bash -c "source $HOME/custom_bridge_ros1_ws/devel/setup.bash &&\
+    source $HOME/ros2_ws/install/setup.bash &&\
+    source $HOME/custom_bridge_ros2_ws/install/setup.bash &&\
+    colcon build --packages-select ros1_bridge --cmake-force-configure --parallel-workers 12"
 
 

custom_bridge_ros1_ws/src/custom_bridge_msgs/CMakeLists.txt

@@ -23,15 +23,15 @@ find_package(catkin REQUIRED COMPONENTS
 ## Generate messages in the 'msg' folder
 add_message_files(
   FILES
-  Intent.msg
-  IntentComponent.msg
+  State.msg
+  CustomState.msg
 )
 
 ## Generate services in the 'srv' folder
 add_service_files(
   FILES
-  BeginPlanning.srv
-  CustomSrvRes.srv
+  CustomServiceOne.srv
+  CustomServiceTwo.srv
 )
 
 ## Generate added messages and services with any dependencies listed here
@@ -55,19 +55,18 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
+# Declare a C++ executable
+add_executable(custom_talker src/custom_talker.cpp)
+add_executable(custom_service_one src/custom_service_one.cpp)
 
+# Specify the dependencies for this executable
+add_dependencies(custom_talker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(custom_service_one ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-# # Declare a C++ executable
-# add_executable(dummy_start_planning_service src/custom_service.cpp)
-
-# # Specify the dependencies for this executable
-# add_dependencies(dummy_start_planning_service ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-
-# target_link_libraries(dummy_start_planning_service ${catkin_LIBRARIES})
-
-# # Install the executable
-# install(TARGETS dummy_start_planning_service
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+target_link_libraries(custom_talker ${catkin_LIBRARIES})
+target_link_libraries(custom_service_one ${catkin_LIBRARIES})
 
+# Install the executable
+install(TARGETS custom_talker custom_service_one
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

custom_bridge_ros1_ws/src/custom_bridge_msgs/README.md

-# custom_bridge_msgs
-
-## custom_bridge_msgs for ros1 (and ros1_bridge)
-
-mount this directory into custom jammy docker image to bridge custom interfaces on 22.04
-

custom_bridge_ros1_ws/src/custom_bridge_msgs/msg/CustomState.msg

+State state
\ No newline at end of file

custom_bridge_ros1_ws/src/custom_bridge_msgs/msg/Intent.msg

-std_msgs/Header header
-IntentComponent[] components
-

custom_bridge_ros1_ws/src/custom_bridge_msgs/msg/IntentComponent.msg

-float32 prob
-nav_msgs/Path path
\ No newline at end of file

custom_bridge_ros1_ws/src/custom_bridge_msgs/msg/State.msg

+float64[] rewards
+int64 value
\ No newline at end of file

custom_bridge_ros1_ws/src/custom_bridge_msgs/src/custom_service.cpp

-// #include "ros/ros.h"
-// #include "custom_bridge_msgs/BeginPlanning.h"
-
-// class DummyStartPlanning
-// {
-// public:
-//     DummyStartPlanning()
-//     {
-//         service_ = nh_.advertiseService("dummy_start_planning", &DummyStartPlanning::handle_service, this);
-//     }
-
-// private:
-//     bool handle_service(custom_bridge_msgs::BeginPlanning::Request &request, custom_bridge_msgs::BeginPlanning::Response &response)
-//     {
-//         ROS_INFO_STREAM("Received request: " << request.start_pose.pose.position.x);
-//         response.start = true;
-//         // response.message = std::to_string(request.spose); // Commented out as in the original code
-//         return true;
-//     }
-
-//     ros::NodeHandle nh_;
-//     ros::ServiceServer service_;
-// };
-
-// int main(int argc, char **argv)
-// {
-//     ros::init(argc, argv, "dummy_start_planning");
-//     DummyStartPlanning dummyStartPlanning;
-//     ros::spin();
-//     return 0;
-// }
-

custom_bridge_ros1_ws/src/custom_bridge_msgs/src/custom_service_one.cpp

+#include "ros/ros.h"
+#include "custom_bridge_msgs/CustomServiceOne.h"
+
+class DummyCustomServiceOne
+{
+public:
+    DummyCustomServiceOne()
+    {
+        service_ = nh_.advertiseService("custom_service_topic", &DummyCustomServiceOne::handle_service, this);
+    }
+
+private:
+    bool handle_service(custom_bridge_msgs::CustomServiceOne::Request &request, custom_bridge_msgs::CustomServiceOne::Response &response)
+    {
+        ROS_INFO_STREAM("Received request: " << request.start_pose.pose.position.x);
+        response.start = true;
+        // response.message = std::to_string(request.spose); // Commented out as in the original code
+        return true;
+    }
+
+    ros::NodeHandle nh_;
+    ros::ServiceServer service_;
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "dummy_custom_service_one");
+    DummyCustomServiceOne dummy_custom_service_one;
+    ros::spin();
+    return 0;
+}
+

custom_bridge_ros1_ws/src/custom_bridge_msgs/src/custom_talker.cpp

+// #include "ros/ros.h"
+// #include "std_msgs/String.h"
+
+// class DummyTalker
+// {
+// public:
+//     DummyTalker() : count_(0)
+//     {
+//         nh_ = ros::NodeHandle("~");
+//         publisher_ = nh_.advertise<std_msgs::String>("chatter", 10);
+//         timer_ = nh_.createTimer(ros::Duration(0.5), &DummyTalker::publish_message, this);
+//     }
+
+// private:
+//     void publish_message(const ros::TimerEvent&)
+//     {
+//         std_msgs::String message;
+//         message.data = "I am talking";
+//         ROS_INFO("Publishing: '%s'", message.data.c_str());
+//         publisher_.publish(message);
+//     }
+
+//     ros::NodeHandle nh_;
+//     ros::Publisher publisher_;
+//     ros::Timer timer_;
+//     size_t count_;
+// };
+
+// int main(int argc, char **argv)
+// {
+//     ros::init(argc, argv, "dummy_talker");
+//     DummyTalker talker;
+//     ros::spin();
+//     return 0;
+// }
+
+
+#include "ros/ros.h"
+#include "custom_bridge_msgs/CustomState.h"
+#include "custom_bridge_msgs/State.h"
+
+int PUBLISH_RATE = 1;
+
+class DummyPublisher
+{
+    public:
+        DummyPublisher() : publish_rate_(PUBLISH_RATE) // Rate of 10 Hz
+        {
+            // Advertise the topic with a queue size of 10
+            publisher_ = nh_.advertise<custom_bridge_msgs::CustomState>("custom_state_topic", PUBLISH_RATE);
+        }
+
+        void publishLoop()
+        {
+            while (ros::ok())
+            {
+                std::vector<double> reward_vector{1., 2., 3., 4., 5.};
+                custom_bridge_msgs::State state;
+                custom_bridge_msgs::CustomState custom_state;
+                state.value = 2;
+                state.rewards = reward_vector;
+                custom_state.state = state;
+                publisher_.publish(custom_state);
+
+                ros::spinOnce();
+                publish_rate_.sleep();
+            }
+        }
+
+    private:
+        ros::NodeHandle nh_;
+        ros::Publisher publisher_;
+        ros::Rate publish_rate_; // Rate object to control the publish frequency
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "dummy_publisher");
+    DummyPublisher dummyPublisher;
+    dummyPublisher.publishLoop();
+    return 0;
+}
\ No newline at end of file

custom_bridge_ros2_ws/src/custom_bridge_msgs/srv/CustomSrvRes.srv → custom_bridge_ros1_ws/src/custom_bridge_msgs/srv/CustomServiceTwo.srv

@@ -2,5 +2,5 @@
 ---
 bool success
 float32 reward
-int32   num_iterations
+int32   iterations
 nav_msgs/Path path
\ No newline at end of file

custom_bridge_ros1_ws/src/custom_bridge_msgs/srv/CustomSrvRes.srv

-# decmcts_msgs/CompletePlanning
-# https://gitlab.com/dst-uts-aerial/decmcts_msgs/-/blob/master/srv/CompletePlanning.srv
-
----
-bool success
-float32 reward
-int32   num_iterations
-nav_msgs/Path path
\ No newline at end of file

custom_bridge_ros2_ws/src/custom_bridge_msgs/CMakeLists.txt

@@ -5,24 +5,35 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-#set(CMAKE_CXX_FLAGS "--std=c++20 ${CMAKE_CXX_FLAGS} -g -rdynamic")
 
-# Find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
-find_package(nav_msgs REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(visualization_msgs REQUIRED)
+set(pkg_dependencies
+  ament_cmake
+  geometry_msgs
+  nav_msgs
+  std_msgs
+  visualization_msgs
+  rosidl_default_generators
+  rclcpp
+  rclcpp_components
+  geometry_msgs
+)
+
+foreach(pkg IN ITEMS ${pkg_dependencies})
+  find_package(${pkg} REQUIRED)
+endforeach()
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/Intent.msg"
-  "msg/IntentComponent.msg"
-  "srv/BeginPlanning.srv"
-  "srv/CustomSrvRes.srv"
+  "msg/State.msg"
+  "msg/CustomState.msg"
+  "srv/CustomServiceOne.srv"
+  "srv/CustomServiceTwo.srv"
   DEPENDENCIES nav_msgs
 )
 
+set(msg_dependencies
+  rclcpp
+  geometry_msgs   # needed by CustomServiceRequest
+)
 
 # Include directories
 include_directories(
@@ -31,12 +42,44 @@ include_directories(
 )
 
 
+rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
 
+# include act/msg/srv files in source code
+include_directories(
+  ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp
+)
+
+
+# Create ROS 2 executables
+add_executable(custom_listener
+  "src/custom_listener.cpp"
+)
+
+ament_target_dependencies(custom_listener
+  ${pkg_dependencies}
+  ${msg_dependencies}
+)
+
+target_link_libraries(custom_listener "${cpp_typesupport_target}") 
+
+
+
+
+
+add_executable(custom_service_two
+  "src/custom_service_two.cpp"
+)
+
+ament_target_dependencies(custom_service_two
+  ${pkg_dependencies}
+  ${msg_dependencies}
+)
+target_link_libraries(custom_service_two "${cpp_typesupport_target}") 
+
 
 
-# Export dependencies for downstream packages
 ament_export_dependencies(rosidl_default_runtime)
 
-# Finalize the package setup
-ament_package()
+
+ament_package()
\ No newline at end of file

custom_bridge_ros2_ws/src/custom_bridge_msgs/msg/CustomState.msg

+State state
\ No newline at end of file

custom_bridge_ros2_ws/src/custom_bridge_msgs/msg/Intent.msg

-std_msgs/Header header
-IntentComponent[] components
-

custom_bridge_ros2_ws/src/custom_bridge_msgs/msg/IntentComponent.msg

-float32 prob
-nav_msgs/Path path
\ No newline at end of file

custom_bridge_ros2_ws/src/custom_bridge_msgs/msg/State.msg

+float64[] rewards
+int64 value
\ No newline at end of file

custom_bridge_ros2_ws/src/custom_bridge_msgs/src/custom_listener.cpp

+#include "rclcpp/rclcpp.hpp"
+#include "custom_bridge_msgs/msg/custom_state.hpp" // Adjust the include path to your ROS 2 message definition
+
+class DummySubscriber : public rclcpp::Node
+{
+public:
+    DummySubscriber() : Node("dummy_subscriber")
+    {
+        subscriber_ = this->create_subscription<custom_bridge_msgs::msg::CustomState>(
+            "custom_state_topic", 10,
+            std::bind(&DummySubscriber::topic_callback, this, std::placeholders::_1));
+    }
+
+private:
+    void topic_callback(const custom_bridge_msgs::msg::CustomState::SharedPtr msg) const
+    {
+        RCLCPP_INFO_STREAM(this->get_logger(), "Received state value: " << msg->state.value);
+        for (int reward : msg->state.rewards)
+        {
+            RCLCPP_INFO_STREAM(this->get_logger(), "reward: " << reward);
+        }
+    }
+
+    rclcpp::Subscription<custom_bridge_msgs::msg::CustomState>::SharedPtr subscriber_;
+};
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<DummySubscriber>());
+    rclcpp::shutdown();
+    return 0;
+}