Merge pull request #52 from lge-ros2/main

Merge 'main' branch into 'foxy'

README.md

@@ -4,15 +4,14 @@ ROS2 simulation device packages to connect CLOiSim(the unity3D based multi-robot
 
 ## Download CLOiSim Simulator
 
-  - Latest version: [link](https://github.com/lge-ros2/cloisim/releases/latest)
-  - All Releases: [link](https://github.com/lge-ros2/cloisim/releases)
+- Latest version: [link](https://github.com/lge-ros2/cloisim/releases/latest)
+- All Releases: [link](https://github.com/lge-ros2/cloisim/releases)
 
 ## Install ROS2 foxy
 
   follow the guideline on below link.
-
-  https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html
 
+  <https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html>
 
 ## Prerequisite
 
@@ -61,7 +60,7 @@ export FASTRTPS_DEFAULT_PROFILES_FILE=/home/cloi/src/cloisim_ros/fastrtps_shared
 
 Apply namespaceas each robot as a multi robot mode.
 
-**(Strongly recommend to use this method.)**
+**Strongly recommend to use this method.**
 
 ```shell
 ros2 launch cloisim_ros_bringup bringup.launch.py

cloisim_ros_base/src/base.cpp

@@ -401,7 +401,7 @@ msgs::Param Base::RequestReplyMessage(zmq::Bridge* const bridge_ptr, const msgs:
 
 msgs::Pose Base::IdentityPose()
 {
-  static msgs::Pose identityTransform;
+  msgs::Pose identityTransform;
   identityTransform.mutable_position()->set_x(0.0);
   identityTransform.mutable_position()->set_y(0.0);
   identityTransform.mutable_position()->set_z(0.0);

cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp

@@ -28,11 +28,12 @@ namespace cloisim_ros
   class BringUpParam : public rclcpp::Node
   {
   private:
-    const int maxRetryNum = 30;
+    const int maxRetryNum = 10;
     const int waitingSeconds = 3;
 
   public:
     BringUpParam(const string basename = "cloisim_ros");
+    virtual ~BringUpParam();
 
     bool IsSingleMode() const { return is_single_mode; }
     string TargetModel() const { return target_model; }

cloisim_ros_bringup_param/src/bringup_param.cpp

@@ -53,6 +53,11 @@ BringUpParam::BringUpParam(const string basename)
   RequestBringUpList();
 }
 
+BringUpParam::~BringUpParam()
+{
+  delete ws_service_ptr_;
+}
+
 void BringUpParam::RequestBringUpList()
 {
   Json::Reader reader;
@@ -113,7 +118,7 @@ Json::Value BringUpParam::GetFilteredListByParameters(const Json::Value result)
 
 Json::Value BringUpParam::GetBringUpList(const bool filterByParameters)
 {
-  return (filterByParameters)? GetFilteredListByParameters(result_map_):result_map_;
+  return (filterByParameters) ? GetFilteredListByParameters(result_map_) : result_map_;
 }
 
 void BringUpParam::StoreBridgeInfosAsParameters(const Json::Value item, rclcpp::NodeOptions &node_options)

cloisim_ros_joint_control/README.md

@@ -19,3 +19,18 @@ or
 ```shell
 ros2 run cloisim_ros_joint_control standalone --ros-args -p target_model:=cloi1 -p target_parts_name:=joint_control
 ```
+
+
+## Example Usage
+
+### state
+
+```shell
+ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["FnB_Display::link"], displacements: [-1.5708], velocities: [2]}'
+```
+
+### command
+
+```shell
+ros2 topic pub -1 /FnB/joint_command control_msgs/msg/JointJog '{joint_names: ["FnB_Display::link"], displacements: [-1.5708], velocities: [2]}'
+```

cloisim_ros_joint_control/src/joint_control.cpp

@@ -95,7 +95,7 @@ void JointControl::Initialize()
 
 string JointControl::MakeCommandMessage(const string joint_name, const double joint_displacement, const double joint_velocity) const
 {
-  static msgs::JointCmd jointCmd;
+  msgs::JointCmd jointCmd;
 
   jointCmd.set_name(joint_name);
 
@@ -105,7 +105,7 @@ string JointControl::MakeCommandMessage(const string joint_name, const double jo
   auto velocity = jointCmd.mutable_velocity();
   velocity->set_target(joint_velocity);
 
-  static string message;
+  string message;
   jointCmd.SerializeToString(&message);
   return message;
 }

cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp

@@ -15,6 +15,8 @@
 #ifndef _CLOISIM_ROS_WEBSOCKET_SERVICE_HPP_
 #define _CLOISIM_ROS_WEBSOCKET_SERVICE_HPP_
 
+#define ASIO_STANDALONE
+
 #include <websocketpp/config/asio_no_tls_client.hpp>
 #include <websocketpp/client.hpp>
 

cloisim_ros_websocket_service/src/websocket_service.cpp

@@ -51,6 +51,7 @@ WebSocketService::WebSocketService(const string bridge_ip, const string service_
   // Register our message handler
   c.set_open_handler(bind(&WebSocketService::on_open, this, placeholders::_1));
   c.set_message_handler(bind(&WebSocketService::on_message, this, placeholders::_1, placeholders::_2));
+  // cout << "WebsocketService created" << endl;
 }
 
 void WebSocketService::on_message(websocketpp::connection_hdl hdl, client::message_ptr msg)
@@ -65,19 +66,22 @@ void WebSocketService::on_open(websocketpp::connection_hdl hdl)
 {
   const string request_msg = "{'command':'device_list', 'filter':'" + target_filter + "'}";
 
-  // std::cout << "Open: " << std::endl;
+  // std::cout << "Open: " << request_msg << std::endl;
   c.send(hdl, request_msg, websocketpp::frame::opcode::value::TEXT);
 }
 
 string WebSocketService::Run()
 {
+  const auto target_uri = uri + "/control";
+  // cout << target_uri << endl;
   try
   {
     websocketpp::lib::error_code ec;
-    client::connection_ptr con = c.get_connection(uri + "/control", ec);
+    auto con = c.get_connection(target_uri, ec);
+
     if (ec)
     {
-      cout << "could not create connection because: " << ec.message() << endl;
+      cout << "could not create connection because: " << ec.message() << " target_uri: " << target_uri << endl;
     }
     else
     {
@@ -97,8 +101,16 @@ string WebSocketService::Run()
   {
     cout << "Exception:" << e.what() << endl;
   }
+  catch (websocketpp::lib::error_code e)
+  {
+    std::cout << e.message() << std::endl;
+  }
+  catch (...)
+  {
+    std::cout << "other exception" << std::endl;
+  }
 
-  // cout << result << endl << result.size() << endl;
+  // cout << payload << endl << payload.size() << endl;
 
   return payload;
 }