diff --git a/cloisim_ros_actor/package.xml b/cloisim_ros_actor/package.xml index 2cf80737..d9f1ffa4 100644 --- a/cloisim_ros_actor/package.xml +++ b/cloisim_ros_actor/package.xml @@ -2,7 +2,7 @@ cloisim_ros_actor - 3.3.0 + 3.3.1 node for actor plugin Hyunseok Yang diff --git a/cloisim_ros_base/include/cloisim_ros_base/base.hpp b/cloisim_ros_base/include/cloisim_ros_base/base.hpp index d7ed4d1e..9a731809 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/base.hpp +++ b/cloisim_ros_base/include/cloisim_ros_base/base.hpp @@ -46,7 +46,6 @@ class Base : public rclcpp::Node protected: void SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const std::string child_frame_id, const std::string header_frame_id = "base_link"); void SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const cloisim::msgs::Pose transform, const std::string child_frame_id, const std::string header_frame_id = "base_link"); - void SetStaticTf2(const std::string child_frame_id, const std::string header_frame_id); void SetStaticTf2(const cloisim::msgs::Pose transform, const std::string parent_header_frame_id = "base_link"); void Start(const bool enable_tf_publish = true); diff --git a/cloisim_ros_base/package.xml b/cloisim_ros_base/package.xml index 08ff1b14..681d3e2e 100644 --- a/cloisim_ros_base/package.xml +++ b/cloisim_ros_base/package.xml @@ -2,7 +2,7 @@ cloisim_ros_base - 3.3.0 + 3.3.1 CLOiSim-ROS base class for other CLOiSim-ROS Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index 7d298c3c..9fd072a4 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/cloisim_ros_base/src/base.cpp @@ -283,13 +283,6 @@ void Base::SetTf2(geometry_msgs::msg::TransformStamped& target_msg, const msgs:: SetQuaternionMessageToGeometry(transform.orientation(), target_msg.transform.rotation); } -void Base::SetStaticTf2(const string child_frame_id, const string header_frame_id) -{ - auto emptyPose = IdentityPose(); - emptyPose.set_name(child_frame_id); - SetStaticTf2(emptyPose, header_frame_id); -} - void Base::SetStaticTf2(const msgs::Pose transform, const string parent_header_frame_id) { geometry_msgs::msg::TransformStamped static_tf; diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp index 2df5afb8..3fb367c9 100644 --- a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp +++ b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/bridge.hpp @@ -16,7 +16,7 @@ #ifndef _CLOISIM_ROS_BRIDGE_ZMQ_HPP_ #define _CLOISIM_ROS_BRIDGE_ZMQ_HPP_ -#include "debug_log.h" +#include "log.h" #include #include @@ -76,8 +76,8 @@ class Bridge zmq_msg_t m_msgRx; // for subscriber and reply std::size_t m_nHashTagTx; // for publisher and request - void* pSockTx_; // for Send function - void* pSockRx_; // for Recieve function + void* pSockTx_; // for Send function + void* pSockRx_; // for Recieve function std::string lastErrMsg; diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/debug_log.h b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/debug_log.h deleted file mode 100644 index afca7c50..00000000 --- a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/debug_log.h +++ /dev/null @@ -1,30 +0,0 @@ -/** - * @file debug_log.h - * @date 2020-04-21 - * @author Hyunseok Yang - * @brief - * the following are UBUNTU/LINUX, and MacOS ONLY terminal color codes. - * @remark - * @copyright - * LGE Advanced Robotics Laboratory - * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea - * All Rights are Reserved. - * - * SPDX-License-Identifier: MIT - */ -#ifndef _DEBUG_LOG_H_ -#define _DEBUG_LOG_H_ - -#include "term_color.h" - -#pragma GCC system_header - -#define _DBG_SIM(COLOR_CODE, STR_FORMAT, ...) \ - printf(COLOR_CODE "[%s][%d] " STR_FORMAT RESET "\n", __FUNCTION__, __LINE__, ## __VA_ARGS__) - -#define DBG_SIM_INFO(STR_FORMAT, ...) _DBG_SIM(CYAN, STR_FORMAT, ## __VA_ARGS__) -#define DBG_SIM_WRN(STR_FORMAT, ...) _DBG_SIM(YELLOW, STR_FORMAT, ## __VA_ARGS__) -#define DBG_SIM_MSG(STR_FORMAT, ...) _DBG_SIM(GREEN, STR_FORMAT, ## __VA_ARGS__) -#define DBG_SIM_ERR(STR_FORMAT, ...) _DBG_SIM(BOLDRED, STR_FORMAT, ## __VA_ARGS__) - -#endif \ No newline at end of file diff --git a/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h new file mode 100644 index 00000000..beb8b68e --- /dev/null +++ b/cloisim_ros_bridge_zmq/include/cloisim_ros_bridge_zmq/log.h @@ -0,0 +1,31 @@ +/** + * @file log.h + * @date 2020-04-21 + * @author Hyunseok Yang + * @brief + * the following are UBUNTU/LINUX, and MacOS ONLY terminal color codes. + * @remark + * @copyright + * LGE Advanced Robotics Laboratory + * Copyright (c) 2020 LG Electronics Inc., LTD., Seoul, Korea + * All Rights are Reserved. + * + * SPDX-License-Identifier: MIT + */ +#ifndef _LOG_H_ +#define _LOG_H_ + +#include "term_color.h" + +#pragma GCC system_header + +#define __SIM_LOG(COLOR_CODE, STR_FORMAT, ...) \ + printf(COLOR_CODE "[%s] " STR_FORMAT RESET "\n", __FUNCTION__, ##__VA_ARGS__) +// printf(COLOR_CODE "[%s][%d] " STR_FORMAT RESET "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__) + +#define DBG_SIM_INFO(STR_FORMAT, ...) __SIM_LOG(CYAN, STR_FORMAT, ##__VA_ARGS__) +#define DBG_SIM_WRN(STR_FORMAT, ...) __SIM_LOG(YELLOW, STR_FORMAT, ##__VA_ARGS__) +#define DBG_SIM_MSG(STR_FORMAT, ...) __SIM_LOG(GREEN, STR_FORMAT, ##__VA_ARGS__) +#define DBG_SIM_ERR(STR_FORMAT, ...) __SIM_LOG(BOLDRED, STR_FORMAT, ##__VA_ARGS__) + +#endif \ No newline at end of file diff --git a/cloisim_ros_bridge_zmq/package.xml b/cloisim_ros_bridge_zmq/package.xml index 9ed30871..d671fa9d 100644 --- a/cloisim_ros_bridge_zmq/package.xml +++ b/cloisim_ros_bridge_zmq/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bridge_zmq - 3.3.0 + 3.3.1 bridge for cloisim(simulator) connection through ZMQ Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_bringup/package.xml b/cloisim_ros_bringup/package.xml index de8f0fa8..1cefbc15 100644 --- a/cloisim_ros_bringup/package.xml +++ b/cloisim_ros_bringup/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup - 3.3.0 + 3.3.1 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_bringup_param/package.xml b/cloisim_ros_bringup_param/package.xml index 6cc08b53..a085491a 100644 --- a/cloisim_ros_bringup_param/package.xml +++ b/cloisim_ros_bringup_param/package.xml @@ -2,7 +2,7 @@ cloisim_ros_bringup_param - 3.3.0 + 3.3.1 Bringup scripts and configurations for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_camera/package.xml b/cloisim_ros_camera/package.xml index ff49ad7b..09c0a58d 100644 --- a/cloisim_ros_camera/package.xml +++ b/cloisim_ros_camera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_camera - 3.3.0 + 3.3.1 virtual camera for cloisim Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_depthcamera/package.xml b/cloisim_ros_depthcamera/package.xml index 4ac94499..51c4ace1 100644 --- a/cloisim_ros_depthcamera/package.xml +++ b/cloisim_ros_depthcamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_depthcamera - 3.3.0 + 3.3.1 virtual depth camera for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_elevator_system/package.xml b/cloisim_ros_elevator_system/package.xml index a2ed91a9..d3bbf4d3 100644 --- a/cloisim_ros_elevator_system/package.xml +++ b/cloisim_ros_elevator_system/package.xml @@ -2,7 +2,7 @@ cloisim_ros_elevator_system - 3.3.0 + 3.3.1 elevator system for simulation Sungkyu Kang diff --git a/cloisim_ros_gps/package.xml b/cloisim_ros_gps/package.xml index 206a7caf..9254bc6d 100644 --- a/cloisim_ros_gps/package.xml +++ b/cloisim_ros_gps/package.xml @@ -2,7 +2,7 @@ cloisim_ros_gps - 3.3.0 + 3.3.1 virtual gps for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_ground_truth/package.xml b/cloisim_ros_ground_truth/package.xml index 37a5cbec..52c2e889 100644 --- a/cloisim_ros_ground_truth/package.xml +++ b/cloisim_ros_ground_truth/package.xml @@ -2,7 +2,7 @@ cloisim_ros_ground_truth - 3.3.0 + 3.3.1 world plugin to retrieve ground truth Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_imu/package.xml b/cloisim_ros_imu/package.xml index e6fda32e..db7889ce 100644 --- a/cloisim_ros_imu/package.xml +++ b/cloisim_ros_imu/package.xml @@ -2,7 +2,7 @@ cloisim_ros_imu - 3.3.0 + 3.3.1 virtual imu for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_joint_control/package.xml b/cloisim_ros_joint_control/package.xml index 991bee5f..10065c3d 100644 --- a/cloisim_ros_joint_control/package.xml +++ b/cloisim_ros_joint_control/package.xml @@ -2,7 +2,7 @@ cloisim_ros_joint_control - 3.3.0 + 3.3.1 joint_control package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_lidar/package.xml b/cloisim_ros_lidar/package.xml index 734ad215..b7cb721c 100644 --- a/cloisim_ros_lidar/package.xml +++ b/cloisim_ros_lidar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_lidar - 3.3.0 + 3.3.1 virtual lidar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_micom/package.xml b/cloisim_ros_micom/package.xml index 6bf3a258..854a7155 100644 --- a/cloisim_ros_micom/package.xml +++ b/cloisim_ros_micom/package.xml @@ -2,7 +2,7 @@ cloisim_ros_micom - 3.3.0 + 3.3.1 micom package for simulator Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp index 640eb0e0..8757457a 100644 --- a/cloisim_ros_micom/src/micom.cpp +++ b/cloisim_ros_micom/src/micom.cpp @@ -55,13 +55,16 @@ void Micom::Initialize() const auto hashKeyTf = GetTargetHashKey("Tf"); DBG_SIM_INFO("hashKey: info(%s) pub(%s) sub(%s) tf(%s)", hashKeyInfo.c_str(), hashKeyPub.c_str(), hashKeySub.c_str(), hashKeyTf.c_str()); - SetStaticTf2("base_link", "base_footprint"); + { + auto base_link_pose = IdentityPose(); + base_link_pose.set_name("base_link"); + SetStaticTf2(base_link_pose, "base_footprint"); + } info_bridge_ptr = CreateBridge(); if (info_bridge_ptr != nullptr) { info_bridge_ptr->Connect(zmq::Bridge::Mode::CLIENT, portInfo, hashKeyInfo); - GetStaticTransforms(info_bridge_ptr); } @@ -130,7 +133,6 @@ string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) angular_ptr->set_z(msg->angular.z); // DBG_SIM_INFO("%f %f", linear_ptr->x(), angular_ptr->z()); - // m/s velocity input // double vel_left_wheel = (vel_lin - (vel_rot * (0.50f) / 2.0)); // double vel_right_wheel = (vel_lin + (vel_rot * (0.50f) / 2.0)); diff --git a/cloisim_ros_msgs/package.xml b/cloisim_ros_msgs/package.xml index c95ad78a..11837534 100644 --- a/cloisim_ros_msgs/package.xml +++ b/cloisim_ros_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_msgs - 3.3.0 + 3.3.1 interfaces package for cloisim_ros Hyunseok Yang diff --git a/cloisim_ros_multicamera/package.xml b/cloisim_ros_multicamera/package.xml index 0540e52a..5c5e19de 100644 --- a/cloisim_ros_multicamera/package.xml +++ b/cloisim_ros_multicamera/package.xml @@ -2,7 +2,7 @@ cloisim_ros_multicamera - 3.3.0 + 3.3.1 virtual multi-camera for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_protobuf_msgs/package.xml b/cloisim_ros_protobuf_msgs/package.xml index bf109d5a..ce487c07 100644 --- a/cloisim_ros_protobuf_msgs/package.xml +++ b/cloisim_ros_protobuf_msgs/package.xml @@ -2,7 +2,7 @@ cloisim_ros_protobuf_msgs - 3.3.0 + 3.3.1 CLOiSim-ROS interafces for communication between simulator and CLOiSim-ROS Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_realsense/package.xml b/cloisim_ros_realsense/package.xml index 430da494..1b09959d 100644 --- a/cloisim_ros_realsense/package.xml +++ b/cloisim_ros_realsense/package.xml @@ -2,7 +2,7 @@ cloisim_ros_realsense - 3.3.0 + 3.3.1 virtual realsense for simulator Sungkyu Kang Hyunseok Yang diff --git a/cloisim_ros_sonar/package.xml b/cloisim_ros_sonar/package.xml index 866e07d4..31aef515 100644 --- a/cloisim_ros_sonar/package.xml +++ b/cloisim_ros_sonar/package.xml @@ -2,7 +2,7 @@ cloisim_ros_sonar - 3.3.0 + 3.3.1 virtual sonar for simulation Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_websocket_service/package.xml b/cloisim_ros_websocket_service/package.xml index b170fb48..1ef61188 100644 --- a/cloisim_ros_websocket_service/package.xml +++ b/cloisim_ros_websocket_service/package.xml @@ -2,7 +2,7 @@ cloisim_ros_websocket_service - 3.3.0 + 3.3.1 websocket service for cloisim(simulator) connection port control Hyunseok Yang Hyunseok Yang diff --git a/cloisim_ros_world/package.xml b/cloisim_ros_world/package.xml index 066321de..fc518915 100644 --- a/cloisim_ros_world/package.xml +++ b/cloisim_ros_world/package.xml @@ -2,7 +2,7 @@ cloisim_ros_world - 3.3.0 + 3.3.1 Utilities to interface with Unity through ROS. Hyunseok Yang Hyunseok Yang