diff --git a/README.md b/README.md index 8ac7e1df..3c8dcf4b 100644 --- a/README.md +++ b/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 + ## 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 diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index 6ca6843b..296ae87e 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/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); diff --git a/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp b/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp index aa33bfb7..e7dc308f 100644 --- a/cloisim_ros_bringup_param/include/cloisim_ros_bringup_param/bringup_param.hpp +++ b/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; } diff --git a/cloisim_ros_bringup_param/src/bringup_param.cpp b/cloisim_ros_bringup_param/src/bringup_param.cpp index 54d4f201..dfcff89d 100644 --- a/cloisim_ros_bringup_param/src/bringup_param.cpp +++ b/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) diff --git a/cloisim_ros_joint_control/README.md b/cloisim_ros_joint_control/README.md index f6ec8fe1..5f375433 100644 --- a/cloisim_ros_joint_control/README.md +++ b/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]}' +``` diff --git a/cloisim_ros_joint_control/src/joint_control.cpp b/cloisim_ros_joint_control/src/joint_control.cpp index 61b86230..af211d5b 100644 --- a/cloisim_ros_joint_control/src/joint_control.cpp +++ b/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; } diff --git a/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp b/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp index 26d1278a..feabdc0e 100644 --- a/cloisim_ros_websocket_service/include/cloisim_ros_websocket_service/websocket_service.hpp +++ b/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 #include diff --git a/cloisim_ros_websocket_service/src/websocket_service.cpp b/cloisim_ros_websocket_service/src/websocket_service.cpp index ec275ac3..149dfb47 100644 --- a/cloisim_ros_websocket_service/src/websocket_service.cpp +++ b/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; } \ No newline at end of file