apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

updated find pose

Browse Source
This commit is contained in:
Apoorva Gupta 2023-03-28 17:21:18 +05:30
parent acbddfc828
commit 42814614e3
4 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ros2_ws/src/find-pose/CMakeLists.txt
View File

@ -59,7 +59,7 @@ find_package(sensor_msgs REQUIRED)
find_package(pcl_msgs REQUIRED) find_package(pcl_msgs REQUIRED)
find_package(image_transport REQUIRED) find_package(image_transport REQUIRED)
find_package(message_filters REQUIRED) find_package(message_filters REQUIRED)
find_package(yolov3_msg REQUIRED) find_package(pipe_msgs REQUIRED)
add_executable(convert_pointcloud_to_depth src/convert_pointcloud_to_depth.cpp) add_executable(convert_pointcloud_to_depth src/convert_pointcloud_to_depth.cpp)
ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge pcl_conversions image_transport message_filters) ament_target_dependencies(convert_pointcloud_to_depth ${Libraries} rclcpp_components rclcpp sensor_msgs cv_bridge pcl_conversions image_transport message_filters)
@ -78,7 +78,7 @@ include_directories(${Qt5Core_INCLUDE_DIRS} ${Qt5Widgets_INCLUDE_DIRS})
#ADD_LIBRARY(find_object ${SRC_FILES}) #ADD_LIBRARY(find_object ${SRC_FILES})
add_executable(find_pose_node src/FindObjectROS.cpp src/find_pose_node.cpp) add_executable(find_pose_node src/FindObjectROS.cpp src/find_pose_node.cpp)
set_target_properties(find_pose_node PROPERTIES OUTPUT_NAME find_pose_node) set_target_properties(find_pose_node PROPERTIES OUTPUT_NAME find_pose_node)
ament_target_dependencies(find_pose_node rclcpp rclcpp_components yolov3_msg cv_bridge sensor_msgs image_transport message_filters tf2_msgs tf2_geometry_msgs ${AMENT_LIBRARIES}) ament_target_dependencies(find_pose_node rclcpp rclcpp_components pipe_msgs cv_bridge sensor_msgs image_transport message_filters tf2_msgs tf2_geometry_msgs ${AMENT_LIBRARIES})
target_link_libraries(find_pose_node Qt5::Core Qt5::Widgets ${OpenCV_LIBRARIES} ${LIBRARIES}) target_link_libraries(find_pose_node Qt5::Core Qt5::Widgets ${OpenCV_LIBRARIES} ${LIBRARIES})

10
ros2_ws/src/find-pose/include/find-pose/FindObjectROS.h
View File

@ -49,8 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/sync_policies/approximate_time.h> #include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h> #include <message_filters/sync_policies/exact_time.h>
#include <sensor_msgs/msg/camera_info.hpp> #include <sensor_msgs/msg/camera_info.hpp>
#include "yolov3_msg/msg/bounding_boxes.hpp" #include "pipe_msgs/msg/bounding_boxes.hpp"
#include "yolov3_msg/msg/bounding_box.hpp" #include "pipe_msgs/msg/bounding_box.hpp"
class FindObjectROS class FindObjectROS
{ {
@ -71,7 +71,7 @@ private:
void estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg, void estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg,
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes); const pipe_msgs::msg::BoundingBoxes::ConstSharedPtr bboxes);
double distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2); double distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2);
double angular_distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2); double angular_distance(geometry_msgs::msg::TransformStamped point1, geometry_msgs::msg::TransformStamped point2);
@ -85,14 +85,14 @@ private:
image_transport::SubscriberFilter rgbSub_; image_transport::SubscriberFilter rgbSub_;
image_transport::SubscriberFilter depthSub_; image_transport::SubscriberFilter depthSub_;
message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_; message_filters::Subscriber<sensor_msgs::msg::CameraInfo> cameraInfoSub_;
message_filters::Subscriber<yolov3_msg::msg::BoundingBoxes> bboxSub_; message_filters::Subscriber<pipe_msgs::msg::BoundingBoxes> bboxSub_;
bool tf_broadcast_; bool tf_broadcast_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::msg::Image, sensor_msgs::msg::Image,
sensor_msgs::msg::Image, sensor_msgs::msg::Image,
sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo,
yolov3_msg::msg::BoundingBoxes> MyApproxSyncPolicy; pipe_msgs::msg::BoundingBoxes> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;

4
ros2_ws/src/find-pose/package.xml
View File

@ -22,8 +22,8 @@
<exec_depend>cv_bridge</exec_depend> <exec_depend>cv_bridge</exec_depend>
<build_depend>pcl_msgs</build_depend> <build_depend>pcl_msgs</build_depend>
<exec_depend>pcl_msgs</exec_depend> <exec_depend>pcl_msgs</exec_depend>
<build_depend>yolov3_msg</build_depend> <build_depend>pipe_msgs</build_depend>
<exec_depend>yolov3_msg</exec_depend> <exec_depend>pipe_msgs</exec_depend>
<build_depend>image_transport</build_depend> <build_depend>image_transport</build_depend>
<exec_depend>image_transport</exec_depend> <exec_depend>image_transport</exec_depend>
<build_depend>message_filters</build_depend> <build_depend>message_filters</build_depend>

4
ros2_ws/src/find-pose/src/FindObjectROS.cpp
View File

@ -71,7 +71,7 @@ FindObjectROS::FindObjectROS(rclcpp::Node * node) :
void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg, void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSharedPtr rgbMsg,
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg, const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg, const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
const yolov3_msg::msg::BoundingBoxes::ConstSharedPtr bboxes) const pipe_msgs::msg::BoundingBoxes::ConstSharedPtr bboxes)
{ {
if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 && if(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0) depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0)
@ -106,7 +106,7 @@ void FindObjectROS::estimate_pose_on_bag(const sensor_msgs::msg::Image::ConstSha
std::vector<geometry_msgs::msg::TransformStamped> transforms; std::vector<geometry_msgs::msg::TransformStamped> transforms;
for(size_t i = 0; i < num_detections ; i++) for(size_t i = 0; i < num_detections ; i++)
{ {
yolov3_msg::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i]; pipe_msgs::msg::BoundingBox object_bbox = bboxes->bounding_boxes[i];
float object_width = object_bbox.xmax - object_bbox.xmin; float object_width = object_bbox.xmax - object_bbox.xmin;
float object_height = object_bbox.ymax - object_bbox.ymin; float object_height = object_bbox.ymax - object_bbox.ymin;
// top left is the 0,0 // top left is the 0,0