caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 18:21:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into system_statuses

Browse Source
This commit is contained in:
Ken Steinfeldt
2018-01-23 11:07:23 -08:00
parent 8c280158e8 6270f7380f
commit fdc2da59ff
21 changed files with 1394 additions and 499 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
rover/rover_camera/src/rover_camera.cpp
View File

@@ -16,6 +16,8 @@ int main(int argc, char** argv)
int large_image_width;
int large_image_height;
int medium_image_width;
int medium_image_height;
int small_image_width;
int small_image_height;
@@ -27,8 +29,10 @@ int main(int argc, char** argv)
node_handle.param("large_image_width", large_image_width, 1280);
node_handle.param("large_image_height", large_image_height, 720);
node_handle.param("small_image_width", small_image_width, 640);
node_handle.param("small_image_height", small_image_height, 360);
node_handle.param("medium_image_width", medium_image_width, 640);
node_handle.param("medium_image_height", medium_image_height, 360);
node_handle.param("small_image_width", small_image_width, 256);
node_handle.param("small_image_height", small_image_height, 144);
cv::VideoCapture cap(capture_device_path);
@@ -42,32 +46,38 @@ int main(int argc, char** argv)
}
std::string large_image_node_name = "image_" + std::to_string(large_image_width) + "x" + std::to_string(large_image_height);
std::string medium_image_node_name = "image_" + std::to_string(medium_image_width) + "x" + std::to_string(medium_image_height);
std::string small_image_node_name = "image_" + std::to_string(small_image_width) + "x" + std::to_string(small_image_height);
image_transport::ImageTransport full_res_image_transport(node_handle);
image_transport::ImageTransport lower_res_image_transport(node_handle);
image_transport::ImageTransport large_image_transport(node_handle);
image_transport::ImageTransport medium_image_transport(node_handle);
image_transport::ImageTransport small_image_transport(node_handle);
image_transport::Publisher full_size_publisher = full_res_image_transport.advertise(large_image_node_name, 1);
image_transport::Publisher lower_size_publisher = lower_res_image_transport.advertise(small_image_node_name, 1);
image_transport::Publisher large_image_publisher = large_image_transport.advertise(large_image_node_name, 1);
image_transport::Publisher medium_image_publisher = medium_image_transport.advertise(medium_image_node_name, 1);
image_transport::Publisher small_image_publisher = small_image_transport.advertise(small_image_node_name, 1);
cv::Mat image;
cv::Mat image_smaller;
ros::Rate loop_rate(fps + 5);
cv::Mat image_large;
cv::Mat image_medium;
cv::Mat image_small;
ros::Rate loop_rate(fps + 2);
while (ros::ok()) {
cap.read(image);
cap.read(image_large);
if(!image.empty()){
cv::resize(image, image_smaller, cv::Size(small_image_width, small_image_height));
if(!image_large.empty()){
cv::resize(image_large, image_medium, cv::Size(medium_image_width, medium_image_height));
cv::resize(image_medium, image_small, cv::Size(small_image_width, small_image_height));
sensor_msgs::ImagePtr full_res_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
sensor_msgs::ImagePtr lower_res_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_smaller).toImageMsg();
sensor_msgs::ImagePtr large_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_large).toImageMsg();
sensor_msgs::ImagePtr medium_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_medium).toImageMsg();
sensor_msgs::ImagePtr small_image_message = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_small).toImageMsg();
full_size_publisher.publish(full_res_message);
lower_size_publisher.publish(lower_res_message);
large_image_publisher.publish(large_image_message);
medium_image_publisher.publish(medium_image_message);
small_image_publisher.publish(small_image_message);
}
ros::spinOnce();