|
9 | 9 | #include <geometry_msgs/TransformStamped.h> |
10 | 10 |
|
11 | 11 |
|
12 | | -geometry_msgs::Vector3 fromCvVector(cv::Vec3d cv_vector){ |
| 12 | +geometry_msgs::Vector3 fromCvVector(const cv::Vec3d& cv_vector){ |
13 | 13 | geometry_msgs::Vector3 ros_vector; |
14 | 14 | ros_vector.x = cv_vector[0]; |
15 | 15 | ros_vector.y = cv_vector[1]; |
@@ -132,14 +132,14 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){ |
132 | 132 | } |
133 | 133 |
|
134 | 134 |
|
135 | | -bool EOD_ROS::check_time(ros::Time stamp){ |
| 135 | +bool EOD_ROS::check_time(const ros::Time& stamp){ |
136 | 136 | if( frame_sequence == 0) |
137 | 137 | return true; |
138 | 138 | return (stamp - prev_detected_time).toSec() > rate_limit_sec; |
139 | 139 | } |
140 | 140 |
|
141 | 141 |
|
142 | | -bool EOD_ROS::check_lag(ros::Time stamp, double& lag){ |
| 142 | +bool EOD_ROS::check_lag(const ros::Time& stamp, double& lag){ |
143 | 143 | if( allowed_lag_sec == 0) |
144 | 144 | return true; |
145 | 145 | lag = (ros::Time::now() - stamp).toSec(); |
@@ -214,14 +214,12 @@ void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const se |
214 | 214 | return; |
215 | 215 | } |
216 | 216 | cv::Mat depth; |
217 | | - if (depth_image->encoding == sensor_msgs::image_encodings::TYPE_16UC1){ |
218 | | - //ROS_INFO("Depth encoding is 16uc1"); |
| 217 | + if (depth_image->encoding == sensor_msgs::image_encodings::TYPE_16UC1){ |
219 | 218 | depth = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_16UC1)->image; |
220 | 219 | depth.convertTo(depth, CV_32F); |
221 | 220 | depth *= 0.001f; |
222 | 221 | } |
223 | | - else if(depth_image->encoding == sensor_msgs::image_encodings::TYPE_32FC1){ |
224 | | - //ROS_INFO("Depth encoding is 32fc1"); |
| 222 | + else if(depth_image->encoding == sensor_msgs::image_encodings::TYPE_32FC1){ |
225 | 223 | depth = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_32FC1)->image; |
226 | 224 | } |
227 | 225 | else{ |
|
0 commit comments