diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 918afcc..fbb9979 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -1,9 +1,6 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package clpe_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -0.1.1 (2022-08-24) ------------------- -* remove eeprom 0.1.0 (2022-04-20) ------------------ @@ -12,4 +9,4 @@ Changelog for package clpe_ros * /clpe_ros/cam_X/image_raw: The raw image published as sensor_msgs::msg::Image. The default encoding is yuv422. For other supported encodings, see Configuration. * /clpe_ros/cam_X/camera_info: The intrinsics of the camera published as a sensor_msgs::msg::CameraInfo message. * /clpe_ros/cam_X/clpe_camera_info: The intrinsics of the camera published as a clpe_ros_msgs::ClpeCameraInfo message. -* Contributors: Teo Koon Peng, Yadunund Vijay +* Contributors: Teo Koon Peng, Yadunund Vijay \ No newline at end of file diff --git a/README.md b/README.md index 3b932e3..b69a237 100644 --- a/README.md +++ b/README.md @@ -36,7 +36,7 @@ Create workspace ```bash mkdir -p ~/ws_clpe/src cd ~/ws_clpe/ -wget https://raw.githubusercontent.com/canlab-co/clpe_ros/noetic/clpe.repos +wget https://raw.githubusercontent.com/canlab-co/clpe_ros/noetic-AP0202/clpe.repos vcs import src < clpe.repos ``` @@ -70,8 +70,6 @@ roslaunch clpe_ros clpe_ros.launch password:= encoding:=yuv422 By default the driver will publish two topics per camera (X). * /clpe_ros/cam_X/image_raw: The raw image published as `sensor_msgs::Image`. The default encoding is `yuv422`. For other supported encodings, see Configuration below. -* /clpe_ros/cam_X/camera_info: The intrinsics of the camera published as a `sensor_msgs::CameraInfo` message. -* /clpe_ros/cam_X/clpe_camera_info: The intrinsics of the camera published as a clpe_ros_msgs::ClpeCameraInfo message. ## Visualizing in Rviz diff --git a/clpe.repos b/clpe.repos index f8e8d80..5208395 100644 --- a/clpe.repos +++ b/clpe.repos @@ -1,7 +1,7 @@ repositories: - canlab-co/CLPE_G_NVP2650D_SDK: + canlab-co/CLPE_G_AP0202_SDK: type: git - url: https://github.com/canlab-co/CLPE_G_NVP2650D_SDK.git + url: https://github.com/canlab-co/CLPE_G_AP0202_SDK.git version: noetic canlab-co/clpe_ros_msgs: type: git @@ -10,4 +10,4 @@ repositories: canlab-co/clpe_ros: type: git url: https://github.com/canlab-co/clpe_ros.git - version: noetic \ No newline at end of file + version: noetic-AP0202 diff --git a/launch/clpe_ros.launch b/launch/clpe_ros.launch index d4b29f2..b8a6bf3 100644 --- a/launch/clpe_ros.launch +++ b/launch/clpe_ros.launch @@ -2,11 +2,13 @@ + + diff --git a/package.xml b/package.xml index 2f6241f..16bec5c 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ clpe_ros - 0.1.1 + 0.1.0 ROS driver for CANLAB CLPE-G-NVP2650D Apache License 2.0 Teo Koon Peng diff --git a/src/ClpeNode.hpp b/src/ClpeNode.hpp index 2b35630..21869de 100644 --- a/src/ClpeNode.hpp +++ b/src/ClpeNode.hpp @@ -23,10 +23,12 @@ #include #include #include +#include #include #include #include #include +#include #include "errors.hpp" @@ -54,8 +56,14 @@ static constexpr std::array kSupportedEncodings({ "yuv422", }); +static constexpr std::array kSupportedTimeStamps({ + "xavier", + "local", +}); + static constexpr const char* kPassword = "password"; static constexpr const char* kEncoding = "encoding"; +static constexpr const char* kTimeStamp = "timestamp"; static constexpr const char* kCamEnable[] = { "cam_0_enable", "cam_1_enable", "cam_2_enable", "cam_3_enable" }; static constexpr const char* kCamPose[] = { "cam_0_pose", "cam_1_pose", "cam_2_pose", "cam_3_pose" }; static constexpr const char* kCamBaseFrame[] = { "cam_0_frame_id", "cam_1_frame_id", "cam_2_frame_id", @@ -64,8 +72,11 @@ static constexpr const char* kCamQueueSize[] = { "cam_0_image_queue_size", "cam_ "cam_2_image_queue_size", "cam_3_image_queue_size" }; static constexpr const char* kCamLatch[] = { "cam_0_image_latch", "cam_1__image_latch", "cam_2_image_latch", "cam_3_image_latch" }; +static constexpr const char* kCamInfoLatch[] = { "cam_0_info_latch", "cam_1_info_latch", "cam_2_info_latch", + "cam_3_info_latch" }; +static constexpr const char* kCamInfoQueueSize[] = { "cam_0_info_queue_size", "cam_1_info_queue_size", + "cam_2_info_queue_size", "cam_3_info_queue_size" }; -// TODO: docs say 95 bytes, but reference sheet is 107 bytes template class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this> { @@ -139,13 +150,15 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisparam(kCamLatch[i], false); int queue_size = this->param(kCamQueueSize[i], 10); kImagePubs[i] = this->transport_->advertise("cam_" + std::to_string(i) + "/image_raw", queue_size, latch); + bool info_latch = this->param(kCamInfoLatch[i], false); + bool info_queue_size = this->param(kCamInfoQueueSize[i], 10); } // start publishing { ROS_INFO("Preparing camera for streaming"); const auto result = this->clpe_api.Clpe_StartStream( - [](unsigned int cam_id, unsigned char* buffer, unsigned int size, struct timeval* frame_us) -> int { + [](unsigned int cam_id, unsigned char* buffer, unsigned int size, struct timeval* camera_timeStamp) -> int { ROS_DEBUG("got new image for cam_%i", cam_id); // skip all work if there is no subscribers @@ -154,12 +167,19 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisparam(kCamBaseFrame[cam_id], "base_link"); - const ros::Time stamp = ros::Time::now(); - Me::FillImageMsg_(buffer, size, stamp, frame_id, image, Me::kNode_->encoding_); - kImagePubs[cam_id].publish(image); + if(Me::kNode_->timestamp_ == "xavier"){ + sensor_msgs::Image image; + const auto frame_id = Me::kNode_->param(kCamBaseFrame[cam_id], "base_link"); + const ros::Time stamp = ros::Time::now(); + Me::FillImageMsg_Xavier_(buffer, size, camera_timeStamp, frame_id, image, Me::kNode_->encoding_); + kImagePubs[cam_id].publish(image); + }else if(Me::kNode_->timestamp_ != "xavier"){ + sensor_msgs::Image image; + const auto frame_id = Me::kNode_->param(kCamBaseFrame[cam_id], "base_link"); + const ros::Time stamp = ros::Time::now(); + Me::FillImageMsg_Local_(buffer, size, stamp, frame_id, image, Me::kNode_->encoding_); + kImagePubs[cam_id].publish(image); + } return 0; }, static_cast(this->cam_enabled_[0]), static_cast(this->cam_enabled_[1]), @@ -181,11 +201,13 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this transport_; std::string encoding_; + std::string timestamp_; std::array cam_enabled_; explicit ClpeNode(ClpeClientApi&& clpe_api) : ros::NodeHandle("~"), clpe_api(std::move(clpe_api)) { this->encoding_ = this->GetEncoding_(); + this->timestamp_ = this->GetTimeStamp_(); for (int i = 0; i < 4; ++i) { this->cam_enabled_[i] = this->param(kCamEnable[i], true); @@ -219,8 +241,30 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_this(buffer, buffer + size); + image.encoding = sensor_msgs::image_encodings::YUV422; + image.width = 1920; + image.height = 1080; + // assume that each row is same sized. + image.step = size / 1080; + image.is_bigendian = false; + image.header.stamp.sec = camera_timeStamp->tv_sec; + image.header.stamp.nsec = (camera_timeStamp->tv_usec) * 1000; - static void FillImageMsg_(unsigned char* buffer, unsigned int size, const ros::Time& stamp, + if (encoding != "yuv422") + { + auto cv_image = cv_bridge::toCvCopy(image, encoding); + cv_image->toImageMsg(image); + } + } + + static void FillImageMsg_Local_(unsigned char* buffer, unsigned int size, const ros::Time& stamp, const std::string& frame_id, sensor_msgs::Image& image, const std::string& encoding) { image.header.frame_id = frame_id; @@ -241,7 +285,8 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thistoImageMsg(image); } } - + +/* std::error_code GetCameraImage_(int cam_id, sensor_msgs::Image& image) { unsigned char* buffer; @@ -256,7 +301,7 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisencoding_); return kNoError; } - +*/ std::string GetEncoding_() { const auto enc = this->param(kEncoding, "yuv422"); @@ -268,6 +313,17 @@ class ClpeNode : public ros::NodeHandle, public std::enable_shared_from_thisparam(kTimeStamp, "xavier"); + if (std::find(kSupportedTimeStamps.begin(), kSupportedTimeStamps.end(), enc) == kSupportedTimeStamps.end()) + { + ROS_FATAL("Unsupported timestamp"); + exit(-1); + } + return enc; + } + friend class ClpeComponentNode; }; @@ -277,4 +333,5 @@ ClpeNode* ClpeNode::kNode_; template std::unordered_map ClpeNode::kImagePubs; + } // namespace clpe