camera_front.cpp

#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/String.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/opencv.hpp>
#include <topicheaders/topicheaders.h>

#define CAMERA_ID 0  // 0: front camera, 1:  bottom camera

using std::cout;
using std::endl;

int camera_switch_msg = 0;
cv::VideoCapture camera;

void CameraSwitchCallback(const std_msgs::String::ConstPtr& msg) {
    camera_switch_msg = atoi(msg->data.c_str());
    if (camera_switch_msg == CAMERA_ID && !camera.isOpened()) {
        if (camera.open(camera_switch_msg)) {
            ROS_DEBUG("Front camera opened from callback.");
        } else {
            ROS_ERROR("Unable to front bottom camera from callback.");
        }
    } else if (camera_switch_msg != CAMERA_ID && camera.isOpened()) {
        camera.release();
        cout << "Front camera closed successfully from callback." << endl;
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "camera_front");
    ros::NodeHandle node_handle_;
    ros::Rate loop_rate(5);
    ros::Subscriber subscriber_ = node_handle_.subscribe(
        topics::CAMERA_CAM_SWITCH, 1, CameraSwitchCallback);
    image_transport::ImageTransport image_transport_(node_handle_);
    image_transport::Publisher publisher_ = image_transport_.advertise(
        topics::CAMERA_FRONT_RAW_IMAGE, 1);

    sensor_msgs::ImagePtr published_image;
    cv_bridge::CvImage raw_image;
    raw_image.encoding = "bgr8";

    camera.open(CAMERA_ID);
    if (camera.isOpened()) {
        ROS_DEBUG("Front camera opened.");
    } else {
        ROS_ERROR("Unable to open front camera. Shutting down.");
        ros::shutdown();
    }

    while (ros::ok()) {
        if (camera.isOpened()) {
            camera >> raw_image.image;
            published_image = raw_image.toImageMsg();
            publisher_.publish(published_image);
        } else {
            ROS_ERROR("Front camera closed - unable to query frame. \
Shutting down.");
        }
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}