diff --git a/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h b/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h index a455400a..73d33c75 100644 --- a/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h +++ b/cloisim_ros_base/include/cloisim_ros_base/camera_helper.h @@ -34,7 +34,7 @@ static std::string GetImageEncondingType(const uint32_t pixel_format) switch (pixel_format) { case 1: - encoding = sensor_msgs::image_encodings::MONO8; + encoding = sensor_msgs::image_encodings::TYPE_8UC1; break; case 2: diff --git a/cloisim_ros_base/src/base.cpp b/cloisim_ros_base/src/base.cpp index 25e7725d..704a22b7 100644 --- a/cloisim_ros_base/src/base.cpp +++ b/cloisim_ros_base/src/base.cpp @@ -192,7 +192,7 @@ string Base::GetModelName() string Base::GetRobotName() { - bool is_single_mode; + bool is_single_mode = false; get_parameter("single_mode", is_single_mode); string robotName; @@ -213,9 +213,9 @@ msgs::Pose Base::GetObjectTransform(zmq::Bridge* const bridge_ptr, const string const auto reply = RequestReplyMessage(bridge_ptr, "request_transform", target_name); - if (reply.ByteSize() <= 0) + if (reply.ByteSizeLong() <= 0) { - DBG_SIM_ERR("Faild to get object transform, length(%ld)", reply.ByteSize()); + DBG_SIM_ERR("Faild to get object transform, length(%ld)", reply.ByteSizeLong()); } else { @@ -251,9 +251,9 @@ void Base::GetRos2Parameter(zmq::Bridge* const bridge_ptr) const auto reply = RequestReplyMessage(bridge_ptr, "request_ros2"); - if (reply.ByteSize() <= 0) + if (reply.ByteSizeLong() <= 0) { - DBG_SIM_ERR("Faild to get ROS2 common info, length(%ld)", reply.ByteSize()); + DBG_SIM_ERR("Faild to get ROS2 common info, length(%ld)", reply.ByteSizeLong()); } else { @@ -420,4 +420,4 @@ void Base::SetTime(const cloisim::msgs::Time &time) void Base::SetTime(const int32_t seconds, const uint32_t nanoseconds) { m_sim_time = Convert(seconds, nanoseconds); -} \ No newline at end of file +} diff --git a/cloisim_ros_realsense/src/realsense.cpp b/cloisim_ros_realsense/src/realsense.cpp index 270007b5..1e1da32e 100644 --- a/cloisim_ros_realsense/src/realsense.cpp +++ b/cloisim_ros_realsense/src/realsense.cpp @@ -233,13 +233,6 @@ void RealSense::PublishImgData(const zmq::Bridge* const bridge_ptr, const std::s sensor_msgs::fillImage(*msg_img, encoding_arg, rows_arg, cols_arg, step_arg, reinterpret_cast(pb_buf_.image().data().data())); - if ((encoding_arg.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0) || - (encoding_arg.compare(sensor_msgs::image_encodings::TYPE_16SC1) == 0) || - (encoding_arg.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)) - { - msg_img->is_bigendian = true; - } - // Publish camera info auto camera_info_msg = camera_info_managers_[bridge_ptr]->getCameraInfo(); camera_info_msg.header.stamp = GetTime();