Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

vision_msgs in stag_detect CMakelists.txt and use default C++ instead of C++11 #278

Open
wants to merge 9 commits into
base: noetic-devel
Choose a base branch
from
Open

vision_msgs in stag_detect CMakelists.txt and use default C++ instead of C++11 #278

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
2bbd281
use default C++ instead of C++11 which avoids '‘std::shared_mutex’ is…
lucasw Sep 24, 2022
55136ce
use hpp instead of deprecated .h pluginlib headers
lucasw Dec 26, 2022
501b839
use cv::drawFrameAxes instead of aruco::drawAxis
lucasw Apr 20, 2024
8c4b4ff
allow overriding of the tf child frame ids so multiple instances of a…
lucasw May 30, 2024
7b3ae52
put fiducial image in relative namespace instead root so multiple ins…
lucasw May 30, 2024
f5b8009
use a regular image publisher instead of image_transport for the debu…
lucasw May 31, 2024
ec23167
copy header into Detection2D items
lucasw May 31, 2024
e1520df
set the camera frame id as a prefix so no chance of conflicts with ot…
lucasw May 31, 2024
e4ea5f5
factor out duplicate code with setting transforms/poses
lucasw May 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 35 additions & 43 deletions aruco_detect/src/aruco_detect.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include <visualization_msgs/Marker.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include <std_srvs/SetBool.h>
Expand Down Expand Up @@ -95,6 +96,8 @@ class FiducialsNode {

double fiducial_len;

std::string tf_prefix;

bool doPoseEstimation;
bool haveCamInfo;
bool publishFiducialTf;
Expand All @@ -111,7 +114,7 @@ class FiducialsNode {
ros::NodeHandle nh;
ros::NodeHandle pnh;

image_transport::Publisher image_pub;
ros::Publisher image_pub;

// log spam prevention
int prev_detected_count;
Expand Down Expand Up @@ -428,8 +431,8 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg)
reprojectionError);

for (size_t i=0; i<ids.size(); i++) {
aruco::drawAxis(cv_ptr->image, cameraMatrix, distortionCoeffs,
rvecs[i], tvecs[i], (float)fiducial_len);
cv::drawFrameAxes(cv_ptr->image, cameraMatrix, distortionCoeffs,
rvecs[i], tvecs[i], (float)fiducial_len);
if(verbose){
ROS_INFO("Detected id %d T %.2f %.2f %.2f R %.2f %.2f %.2f", ids[i],
tvecs[i][0], tvecs[i][1], tvecs[i][2],
Expand All @@ -456,37 +459,39 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg)
(reprojectionError[i] / dist(corners[i][0], corners[i][2])) *
(norm(tvecs[i]) / fiducial_len);

tf2::Quaternion q;
geometry_msgs::Transform transform;
{
transform.translation.x = tvecs[i][0];
transform.translation.y = tvecs[i][1];
transform.translation.z = tvecs[i][2];
q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
transform.rotation.w = q.w();
transform.rotation.x = q.x();
transform.rotation.y = q.y();
transform.rotation.z = q.z();
}

// Standard ROS vision_msgs
fiducial_msgs::FiducialTransform ft;
tf2::Quaternion q;
if (vis_msgs) {
vision_msgs::Detection2D vm;
vm.header = vma.header;
vision_msgs::ObjectHypothesisWithPose vmh;
vmh.id = ids[i];
vmh.score = exp(-2 * object_error); // [0, infinity] -> [1,0]
vmh.pose.pose.position.x = tvecs[i][0];
vmh.pose.pose.position.y = tvecs[i][1];
vmh.pose.pose.position.z = tvecs[i][2];
q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
vmh.pose.pose.orientation.w = q.w();
vmh.pose.pose.orientation.x = q.x();
vmh.pose.pose.orientation.y = q.y();
vmh.pose.pose.orientation.z = q.z();
vmh.pose.pose.position.x = transform.translation.x;
vmh.pose.pose.position.y = transform.translation.y;
vmh.pose.pose.position.z = transform.translation.z;
vmh.pose.pose.orientation = transform.rotation;

vm.results.push_back(vmh);
vma.detections.push_back(vm);
}
else {
ft.fiducial_id = ids[i];

ft.transform.translation.x = tvecs[i][0];
ft.transform.translation.y = tvecs[i][1];
ft.transform.translation.z = tvecs[i][2];
q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
ft.transform.rotation.w = q.w();
ft.transform.rotation.x = q.x();
ft.transform.rotation.y = q.y();
ft.transform.rotation.z = q.z();
ft.transform = transform;
ft.fiducial_area = calcFiducialArea(corners[i]);
ft.image_error = reprojectionError[i];
// Convert image_error (in pixels) to object_error (in meters)
Expand All @@ -499,28 +504,13 @@ void FiducialsNode::poseEstimateCallback(const FiducialArrayConstPtr & msg)

// Publish tf for the fiducial relative to the camera
if (publishFiducialTf) {
if (vis_msgs) {
geometry_msgs::TransformStamped ts;
ts.transform.translation.x = tvecs[i][0];
ts.transform.translation.y = tvecs[i][1];
ts.transform.translation.z = tvecs[i][2];
ts.transform.rotation.w = q.w();
ts.transform.rotation.x = q.x();
ts.transform.rotation.y = q.y();
ts.transform.rotation.z = q.z();
ts.header.frame_id = frameId;
ts.header.stamp = msg->header.stamp;
ts.child_frame_id = "fiducial_" + std::to_string(ids[i]);
broadcaster.sendTransform(ts);
}
else {
geometry_msgs::TransformStamped ts;
ts.transform = ft.transform;
ts.header.frame_id = frameId;
ts.header.stamp = msg->header.stamp;
ts.child_frame_id = "fiducial_" + std::to_string(ft.fiducial_id);
broadcaster.sendTransform(ts);
}
const auto frame = msg->header.frame_id + "_" + tf_prefix + std::to_string(ids[i]);
geometry_msgs::TransformStamped ts;
ts.transform = transform;
ts.header.frame_id = frameId;
ts.header.stamp = msg->header.stamp;
ts.child_frame_id = frame;
broadcaster.sendTransform(ts);
}
}
}
Expand Down Expand Up @@ -614,6 +604,8 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh)
pnh.param<bool>("vis_msgs", vis_msgs, false);
pnh.param<bool>("verbose", verbose, false);

pnh.param<std::string>("tf_prefix", tf_prefix, "fiducial_");

std::string str;
std::vector<std::string> strs;

Expand Down Expand Up @@ -659,7 +651,7 @@ FiducialsNode::FiducialsNode() : nh(), pnh("~"), it(nh)
}
}

image_pub = it.advertise("/fiducial_images", 1);
image_pub = nh.advertise<sensor_msgs::Image>("fiducial_images", 1);

vertices_pub = nh.advertise<fiducial_msgs::FiducialArray>("fiducial_vertices", 1);

Expand Down
3 changes: 1 addition & 2 deletions stag_detect/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
fiducial_msgs
dynamic_reconfigure
vision_msgs
)

find_package(OpenCV REQUIRED)
Expand All @@ -36,8 +37,6 @@ catkin_package(
## Build ##
###########

add_definitions(-std=c++11)

include_directories(
include
${catkin_INCLUDE_DIRS}
Expand Down
2 changes: 1 addition & 1 deletion stag_detect/include/stag_ros/stag_nodelet.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#pragma once
#include <pluginlib/class_list_macros.h>
#include <pluginlib/class_list_macros.hpp>

#include <nodelet/nodelet.h>

Expand Down
Loading