tracking.cpp
Go to the documentation of this file.
1 /*
2  * LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE
3  */
4 
6 #include <std_msgs/String.h>
7 
8 namespace yocs
9 {
10 
12  std::cout << "ARPairTracking constructor" << std::endl;
13  init();
14 }
15 
17 
19 {
20  ros::NodeHandle pnh("~");
25 
26  /*********************
27  ** Publishers
28  **********************/
29  pub_initial_pose_ = pnh.advertise<geometry_msgs::PoseWithCovarianceStamped>(ARPairTrackingDefaultParams::PUB_INITIAL_POSE, 1);
33 
34  ar_pairs_.clear();
35  return true;
36 }
37 
38 void ARPairTracking::addPair(const yocs_msgs::ARPair& pair)
39 {
40  ar_pairs_.push_back(pair);
41 }
42 
43 void ARPairTracking::updateARPairsCB(const yocs_msgs::ARPairList::ConstPtr& msg)
44 {
45  unsigned int i;
46  ROS_INFO("AR Pair Tracking : New Pairs Received");
47 
48  ar_pairs_.clear();
49 
50  for(i =0; i < msg->pairs.size(); i++)
51  {
52  ar_pairs_.push_back(msg->pairs[i]);
53  }
54 }
55 
56 void ARPairTracking::customCB(const ar_track_alvar_msgs::AlvarMarkers& spotted_markers, const std::vector<TrackedMarker> &tracked_markers)
57 {
58  ar_track_alvar_msgs::AlvarMarker left;
59  ar_track_alvar_msgs::AlvarMarker right;
60  bool both_spotted;
61  unsigned int i;
62 
63  // More than one global Marker Pairs should not be detected in one scene
64  for(i = 0; i < ar_pairs_.size(); i++)
65  {
66  both_spotted = spotMarkerPair(spotted_markers, ar_pairs_[i], left, right);
67 
68  // TODO: use confidence to incorporate covariance to global poses
69  if(both_spotted) {
70  computeRelativeRobotPose(ar_pairs_[i], tracked_markers, left, right);
71  }
72  }
73 }
74 
75 bool ARPairTracking::spotMarkerPair(const ar_track_alvar_msgs::AlvarMarkers& spotted_markers, const yocs_msgs::ARPair& pair, ar_track_alvar_msgs::AlvarMarker& left, ar_track_alvar_msgs::AlvarMarker& right)
76 {
77  bool left_spotted = included(pair.left_id, spotted_markers, &left);
78  bool right_spotted = included(pair.right_id, spotted_markers, &right);
79 
80  // should use a custom message instead of strings (e.g. bool left_spotted, bool right_spotted)
81  std_msgs::String spotted;
82  if(left_spotted && !right_spotted) {
83  spotted.data = "left";
84  } else if(!left_spotted && right_spotted) {
85  spotted.data = "right";
86  } else if(left_spotted && right_spotted) {
87  spotted.data = "both";
88  } else {
89  spotted.data = "none";
90  }
92 
93  return left_spotted && right_spotted;
94 }
95 
96 void ARPairTracking::computeRelativeRobotPose(const yocs_msgs::ARPair& spotted_pair, const std::vector<TrackedMarker>& tracked_markers, const ar_track_alvar_msgs::AlvarMarker& left, const ar_track_alvar_msgs::AlvarMarker& right)
97 {
98  double baseline = spotted_pair.baseline;
99  double target_pose_offset = spotted_pair.target_offset;
100  std::string target_pose_frame = spotted_pair.target_frame;
101  double left_x = left.pose.pose.position.x;
102  double left_z = left.pose.pose.position.z;
103  double right_x = right.pose.pose.position.x;
104  double right_z = right.pose.pose.position.z;
105 
106  double left_d = std::sqrt(left_x*left_x + left_z*left_z);
107  double right_d = std::sqrt(right_x*right_x + right_z*right_z);
108  double b = baseline/2 + (left_d*left_d-right_d*right_d)/(2*baseline);
109  double a = std::sqrt(left_d*left_d - b*b);
110  ROS_DEBUG_STREAM("AR Pairing Tracker : computing robot-marker relative pose");
111  ROS_DEBUG_STREAM("AR Pairing Tracker : left : [" << left_x << "," << left_z << "," << left_d << "]");
112  ROS_DEBUG_STREAM("AR Pairing Tracker : right: [" << right_x << "," << right_z << "," << right_d << "]");
113  ROS_DEBUG_STREAM("AR Pairing Tracker : 1: " << baseline/2);
114  ROS_DEBUG_STREAM("AR Pairing Tracker : 2: " << (left_d*left_d-right_d*right_d)/(2*baseline));
115  ROS_DEBUG_STREAM("AR Pairing Tracker : a=" << a << " b=" << b);
116 
117  // angle between the robot and the first marker
118  double alpha = atan2(left_x, left_z);
119  double alpha_degrees = alpha * (180.0) / M_PI;
120 
121  // alpah + beta is angle between the robot and the second marker
122  double beta = atan2(right_x, right_z);
123  double beta_degrees = beta * (180.0) / M_PI;
124 
125  // theta is the angle between the wall and the perpendicular in front of the robot
126  double theta = atan2((left_z - right_z), (right_x - left_x));
127  double theta_degrees = theta * (180.0) / M_PI;
128 
129  double target_x = left_x + (right_x - left_x) / 2 - target_pose_offset * sin(theta);
130  double target_z = left_z + (right_z - left_z) / 2 - target_pose_offset * cos(theta);
131  double target_heading = atan2(target_x, target_z);
132  double target_heading_degrees = target_heading * 180.0 / M_PI;
133 
134  ROS_DEBUG_STREAM("AR Pairing Tracker : alpha=" << alpha_degrees << "degrees");
135  ROS_DEBUG_STREAM("AR Pairing Tracker : beta=" << beta_degrees << "degrees");
136  ROS_DEBUG_STREAM("AR Pairing Tracker : theta=" << theta_degrees << "degrees");
137  ROS_DEBUG_STREAM("AR Pairing Tracker : t_[x,z,h]=[" << target_x << "," << target_z << "," << target_heading_degrees << "deg]");
138 
139  // target_pose -> camera
140  geometry_msgs::PoseStamped pose;
141  pose.header.frame_id = marker_frame_;
142  pose.pose.position.x = target_x;
143  pose.pose.position.y = 0;
144  pose.pose.position.z = target_z;
145 
146  tf::Quaternion quat;
147  quat.setEuler(theta,0,0);
148 
149  pose.pose.orientation.x = quat.getX();
150  pose.pose.orientation.y = quat.getY();
151  pose.pose.orientation.z = quat.getZ();
152  pose.pose.orientation.w = quat.getW();
153 
154  try
155  {
156  // get and set ar_link -> target_pose
157  tf::StampedTransform tf_ar_target_pose;
158  tf_listener_.lookupTransform(target_pose_frame, global_frame_, ros::Time(0), tf_ar_target_pose);
159  tf_internal_.setTransform(tf_ar_target_pose);
160 
161  // set target_pose -> camera
162  tf::StampedTransform tf_ar_camera;
163  tf_ar_camera.child_frame_id_ = target_pose_frame;
164  mtk::pose2tf(pose, tf_ar_camera); // pose frame_id is set above (camera_rgb_optical_frame)
165  tf_ar_camera.stamp_ = ros::Time::now();
166  tf_internal_.setTransform(tf_ar_camera);
167 
168  // get and set camera -> base_footprint
169  tf::StampedTransform tf_camera_base_footprint;
170  tf_listener_.lookupTransform(base_frame_, marker_frame_, ros::Time(0), tf_camera_base_footprint);
171  tf_internal_.setTransform(tf_camera_base_footprint);
172 
173  // get and publish ar_link -> base_footprint
174  tf::StampedTransform tf_ar_base_footprint;
175  tf_internal_.lookupTransform(global_frame_, base_frame_, ros::Time(0), tf_ar_base_footprint);
176  boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped> pwcs(new geometry_msgs::PoseWithCovarianceStamped);
177 
178  pwcs->header.stamp = tf_ar_base_footprint.stamp_;
179  pwcs->header.frame_id = global_frame_;
180 
181  geometry_msgs::PoseStamped ps;
182  mtk::tf2pose(tf_ar_base_footprint, ps);
183  pwcs->pose.pose = ps.pose;
184  pwcs->pose.covariance[6*0+0] = 0.2 * 0.2;
185  pwcs->pose.covariance[6*1+1] = 0.2 * 0.2;
186  pwcs->pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
187 
188  // publish robot pose to nav watch dog
191 
192  // only for use in standalone mode with a 3d sensor (no robot).
193  if(publish_transforms) {
194  tf_brcaster_.sendTransform(tf_ar_base_footprint);
195  }
196  }
197  catch (tf::TransformException const &ex)
198  {
199  ROS_WARN_STREAM("TF error: " << ex.what());
200  }
201 }
202 
203 } // yocs namespace
void pose2tf(const geometry_msgs::Pose &pose, tf::Transform &tf)
void updateARPairsCB(const yocs_msgs::ARPairList::ConstPtr &msg)
Definition: tracking.cpp:43
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void customCB(const ar_track_alvar_msgs::AlvarMarkers &spotted_markers, const std::vector< TrackedMarker > &tracked_markers)
Definition: tracking.cpp:56
void computeRelativeRobotPose(const yocs_msgs::ARPair &spotted_pair, const std::vector< TrackedMarker > &tracked_markers, const ar_track_alvar_msgs::AlvarMarker &left, const ar_track_alvar_msgs::AlvarMarker &right)
Definition: tracking.cpp:96
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
virtual ~ARPairTracking()
Definition: tracking.cpp:16
std::string marker_frame_
Definition: tracking.hpp:93
TFSIMD_FORCE_INLINE const tfScalar & getW() const
const std::string PUB_RELATIVE_TARGET_POSE
Definition: tracking.hpp:46
ros::Subscriber sub_update_ar_pairs_
Definition: tracking.hpp:82
std::string child_frame_id_
std::vector< yocs_msgs::ARPair > ar_pairs_
Definition: tracking.hpp:89
bool spotted(double younger_than, double min_confidence, ar_track_alvar_msgs::AlvarMarkers &spotted_markers)
const std::string BASE_FRAME
Definition: tracking.hpp:52
bool spotMarkerPair(const ar_track_alvar_msgs::AlvarMarkers &spotted_markers, const yocs_msgs::ARPair &pair, ar_track_alvar_msgs::AlvarMarker &left, ar_track_alvar_msgs::AlvarMarker &right)
Definition: tracking.cpp:75
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void sendTransform(const StampedTransform &transform)
const std::string MARKER_FRAME
Definition: tracking.hpp:51
std::string base_frame_
Definition: tracking.hpp:94
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_WARN_STREAM(args)
tf::TransformBroadcaster tf_brcaster_
Definition: tracking.hpp:86
#define ROS_DEBUG_STREAM(args)
tf::TransformListener tf_listener_
Definition: tracking.hpp:85
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
void setEuler(const tfScalar &yaw, const tfScalar &pitch, const tfScalar &roll)
void addPair(const yocs_msgs::ARPair &p)
Definition: tracking.cpp:38
const std::string PUB_INITIAL_POSE
Definition: tracking.hpp:47
ros::Publisher pub_relative_target_pose_
Definition: tracking.hpp:81
bool included(const uint32_t id, const ar_track_alvar_msgs::AlvarMarkers &v, ar_track_alvar_msgs::AlvarMarker *e=NULL)
std::string global_frame_
Definition: tracking.hpp:92
static Time now()
void tf2pose(const tf::Transform &tf, geometry_msgs::Pose &pose)
tf::Transformer tf_internal_
Definition: tracking.hpp:84
ros::Publisher pub_initial_pose_
Definition: tracking.hpp:81
const std::string SUB_UPDATE_AR_PAIRS
Definition: tracking.hpp:49
const std::string GLOBAL_FRAME
Definition: tracking.hpp:50
ros::Publisher pub_spotted_markers_
Definition: tracking.hpp:81
const std::string PUB_SPOTTED_MARKERS
Definition: tracking.hpp:48


yocs_ar_pair_tracking
Author(s): Daniel Stonier, Jihoon Lee, Jorge Santos Simon
autogenerated on Mon Jun 10 2019 15:53:47