From 63cecf187dd4daae14daa9e9a87613901553b72d Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:40:26 +0400 Subject: [PATCH 01/18] a node for getting stereo images --- src/stereo_node.cpp | 951 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 951 insertions(+) create mode 100644 src/stereo_node.cpp diff --git a/src/stereo_node.cpp b/src/stereo_node.cpp new file mode 100644 index 0000000..ccbcc36 --- /dev/null +++ b/src/stereo_node.cpp @@ -0,0 +1,951 @@ +/* +Copyright (c) 2013, Broadcom Europe Ltd +Copyright (c) 2013, James Hughes +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +#define DBG_MSG(msg) std::cout << msg << std::endl; + +#ifdef __x86_64__ + +#include + +int main(int argc, char** argv) { + (void)fprintf(stderr, "The raspicam_node for the x86/64 architecture is a fake!\n"); + return 1; +} + +#endif // __x86_64__ + +#ifdef __arm__ + +// We use some GNU extensions (basename) +#include +#include +#include +#include + +#include + +#define VCOS_ALWAYS_WANT_LOGGING +#define VERSION_STRING "v1.2" + +#include "bcm_host.h" +#include "interface/vcos/vcos.h" + +#include "interface/mmal/mmal.h" +#include "interface/mmal/mmal_buffer.h" +#include "interface/mmal/mmal_logging.h" +#include "interface/mmal/util/mmal_connection.h" +#include "interface/mmal/util/mmal_default_components.h" +#include "interface/mmal/util/mmal_util.h" +#include "interface/mmal/util/mmal_util_params.h" + +#include "camera_info_manager/camera_info_manager.h" +#include "ros/ros.h" +#include "sensor_msgs/CameraInfo.h" +#include "sensor_msgs/CompressedImage.h" +#include "sensor_msgs/Image.h" +#include "sensor_msgs/SetCameraInfo.h" +#include "std_srvs/Empty.h" + #include "raspicam_node/MotionVectors.h" +#include +#include +extern "C" { +#include "RaspiCamControl.h" +} +#include +#include + +#include "mmal_cxx_helper.h" + +static constexpr int IMG_BUFFER_SIZE = 10 * 1024 * 1024; // 10 MB + +// Video format information +static constexpr int VIDEO_FRAME_RATE_DEN = 1; + +// Video render needs at least 2 buffers. +static constexpr int VIDEO_OUTPUT_BUFFERS_NUM = 3; + +/** Structure containing all state information for the current run + */ +struct RASPIVID_STATE { + RASPIVID_STATE() + : camera_component(nullptr) + , splitter_component(nullptr) + , splitter_connection(nullptr) + , splitter_pool(nullptr, mmal::default_delete_pool){} + bool isInit; + int width; /// Requested width of image + int height; /// requested height of image + int framerate; /// Requested frame rate (fps) + int quality; + + int camera_id = 0; + + RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters + + mmal::component_ptr camera_component; + mmal::component_ptr splitter_component; + + mmal::connection_ptr splitter_connection; /// Pointer to camera => splitter + mmal::pool_ptr splitter_pool; // Pointer buffer pool used by splitter (raw) output + // The Updater class advertises to /diagnostics, and has a + // ~diagnostic_period parameter that says how often the diagnostics + // should be published. + diagnostic_updater::Updater updater; +}; + +/** Struct used to pass information in encoder port userdata to callback + */ +typedef struct MMAL_PORT_USERDATA_T { + MMAL_PORT_USERDATA_T(RASPIVID_STATE& state) : pstate(state){}; + std::unique_ptr buffer[2]; // Memory to write buffer data to. + RASPIVID_STATE& pstate; // pointer to our state for use by callback + bool abort; // Set to 1 in callback if an error occurs to attempt to abort + // the capture + int frame; + int id; + + int frames_skipped = 0; +} PORT_USERDATA; + +using diagnostic_updater::DiagnosedPublisher; +using diagnostic_updater::FrequencyStatusParam; +using diagnostic_updater::TimeStampStatusParam; + +// Helper template +template +struct DiagnosedMsgPublisher { + std::unique_ptr> pub; + T msg; +}; + +static DiagnosedMsgPublisher left_image; +static DiagnosedMsgPublisher right_image; + +ros::Publisher left_camera_info_pub; +ros::Publisher right_camera_info_pub; +sensor_msgs::CameraInfo left_info; +sensor_msgs::CameraInfo right_info; +std::string camera_frame_id; +int skip_frames = 0; + +/** + * Assign a default set of parameters to the state passed in + * + * @param state state structure to assign defaults to + * @param nh NodeHandle to get params from + */ +void print_image(const sensor_msgs::Image& msg) +{ + std::cout << "header { stamp: { sec: " << msg.header.stamp.sec << ", nsec: " << msg.header.stamp.nsec + << ", frameid: " << msg.header.frame_id << " }, \nheight: " << msg.height << "\nwidth: " << msg.width + << "\nstep: " << msg.step << "\nencoding: " << msg.encoding << "\ndata_size: " << msg.data.size() << std::endl; +} +static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { + nh.param("width", state.width, 1280); + nh.param("height", state.height, 720); + state.height*=2; + nh.param("quality", state.quality, 80); + if (state.quality < 0 && state.quality > 100) { + ROS_WARN("quality: %d is outside valid range 0-100, defaulting to 80", state.quality); + state.quality = 80; + } + nh.param("framerate", state.framerate, 15); + if (state.framerate < 0 && state.framerate > 90) { + ROS_WARN("framerate: %d is outside valid range 0-90, defaulting to 30", state.framerate); + state.framerate = 30; + } + + nh.param("camera_frame_id", camera_frame_id, "stereo"); + + nh.param("camera_id", state.camera_id, 0); + + // Set up the camera_parameters to default + raspicamcontrol_set_defaults(&state.camera_parameters); + + bool temp; + nh.param("hFlip", temp, false); + state.camera_parameters.hflip = temp; // Hack for bool param => int variable + nh.param("vFlip", temp, false); + state.camera_parameters.vflip = temp; // Hack for bool param => int variable + nh.param("shutter_speed", state.camera_parameters.shutter_speed, 0); + + state.isInit = false; +} + +/** + * buffer header callback function for image encoder + * + * Callback will dump buffer data to the specific file + * + * @param port Pointer to port from which callback originated + * @param buffer mmal buffer header pointer + */ +void update_image(sensor_msgs::Image& msg, MMAL_BUFFER_HEADER_T* buffer, PORT_USERDATA* pData) +{ + msg.header.frame_id = camera_frame_id; + msg.header.stamp = ros::Time::now(); //(double(buffer->dts)/1e6); + msg.encoding = "bgr8"; +// #ifdef _DEBUG +// DBG_MSG("//DEBUG// - buffer->dts:") +// DBG_MSG( buffer->dts) +// DBG_MSG(MMAL_TIME_UNKNOWN) +// #endif // _DEBUG + msg.is_bigendian = false; + msg.height = pData->pstate.height/2; + msg.width = pData->pstate.width; + msg.step = (pData->pstate.width * 3); +} +static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* buffer) { + // We pass our file handle and other stuff in via the userdata field. + PORT_USERDATA* pData = port->userdata; + if (pData && pData->pstate.isInit) { + size_t bytes_written = buffer->length; + if (buffer->length) { + if (pData->id != INT_MAX) { + if (pData->id + buffer->length > IMG_BUFFER_SIZE) { + ROS_ERROR("pData->id (%d) + buffer->length (%d) > " + "IMG_BUFFER_SIZE (%d), skipping the frame", + pData->id, buffer->length, IMG_BUFFER_SIZE); + pData->id = INT_MAX; // mark this frame corrupted + } else { + mmal_buffer_header_mem_lock(buffer); + memcpy(&(pData->buffer[pData->frame & 1].get()[pData->id]), buffer->data, buffer->length); + pData->id += bytes_written; + mmal_buffer_header_mem_unlock(buffer); + } + } + } + + if (bytes_written != buffer->length) { + vcos_log_error("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length); + ROS_ERROR("Failed to write buffer data (%d from %d)- aborting", bytes_written, buffer->length); + pData->abort = true; + } + + int complete = false; + if (buffer->flags & (MMAL_BUFFER_HEADER_FLAG_FRAME_END | MMAL_BUFFER_HEADER_FLAG_TRANSMISSION_FAILED)) + complete = true; + + if (complete) { + if (pData->id != INT_MAX) { + // ROS_INFO("Frame size %d", pData->id); + if (skip_frames > 0 && pData->frames_skipped < skip_frames) { + pData->frames_skipped++; + } else { + pData->frames_skipped = 0; + update_image(left_image.msg, buffer, pData); + update_image(right_image.msg, buffer, pData); + auto start = pData->buffer[pData->frame & 1].get(); + auto end = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); + auto start1 = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); + auto end1 = &(pData->buffer[pData->frame & 1].get()[pData->id]); + left_image.msg.data.resize(pData->id/2); + #ifdef _DEBUG + DBG_MSG("//DEBUG// - Fill in left image!!!") + #endif // _DEBUG + std::copy(start1, end1, left_image.msg.data.begin()); + #ifdef _DEBUG + DBG_MSG("//DEBUG// - Success") + #endif // _DEBUG + right_image.msg.data.resize(pData->id/2); + #ifdef _DEBUG + DBG_MSG("//DEBUG// - Fill in right image!!!") + #endif // _DEBUG + std::copy(start, end, right_image.msg.data.begin()); + #ifdef _DEBUG + DBG_MSG("//DEBUG// - Success!!!") + DBG_MSG("//DEBUG// - Publishing messages!!!") + print_image(left_image.msg); + print_image(right_image.msg); + #endif // _DEBUG + + left_image.pub->publish(left_image.msg); + right_image.pub->publish(right_image.msg); + + #ifdef _DEBUG + DBG_MSG("//DEBUG// - Success!!!") + #endif // _DEBUG + left_info.header = left_image.msg.header; + right_info.header = left_image.msg.header; + left_camera_info_pub.publish(left_info); + right_camera_info_pub.publish(right_info); + + } + } + pData->frame++; + pData->id = 0; + } + } + + // release buffer back to the pool + mmal_buffer_header_release(buffer); + + // and send one back to the port (if still open) + if (port->is_enabled) { + MMAL_STATUS_T status; + + MMAL_BUFFER_HEADER_T* new_buffer = mmal_queue_get(pData->pstate.splitter_pool->queue); + + if (new_buffer) + status = mmal_port_send_buffer(port, new_buffer); + + if (!new_buffer || status != MMAL_SUCCESS) { + vcos_log_error("Unable to return a buffer to the splitter port"); + ROS_ERROR("Unable to return a buffer to the splitter port"); + } + } +} + +/** + * Create the camera component, set up its ports + * + * @param state Pointer to state control struct + * + * @return 0 if failed, pointer to component if successful + * + */ +static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) { + MMAL_COMPONENT_T* camera = 0; + MMAL_ES_FORMAT_T* format; + MMAL_PORT_T *video_port = nullptr, *preview_port = nullptr, *still_port = nullptr; + MMAL_STATUS_T status; + + /* Create the component */ + status = mmal_component_create(MMAL_COMPONENT_DEFAULT_CAMERA, &camera); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Failed to create camera component"); + ROS_ERROR("Failed to create camera component"); + goto error; + } + + state.camera_parameters.stereo_mode.mode = stereo_mode_from_string("tb"); + status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[0], &state.camera_parameters.stereo_mode)); + if (status != MMAL_SUCCESS) + { + vcos_log_error("Could not set stereo mode : error %d", status); + goto error; + } + status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[1], &state.camera_parameters.stereo_mode)); + + if (status != MMAL_SUCCESS) + { + vcos_log_error("Could not set stereo mode : error %d", status); + goto error; + } + status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[2], &state.camera_parameters.stereo_mode)); + + if (status != MMAL_SUCCESS) + { + vcos_log_error("Could not set stereo mode : error %d", status); + goto error; + } + if (!camera->output_num) { + vcos_log_error("Camera doesn't have output ports"); + ROS_ERROR("Camera doesn't have output ports"); + goto error; + } + + video_port = camera->output[mmal::camera_port::video]; + preview_port = camera->output[mmal::camera_port::preview]; + still_port = camera->output[mmal::camera_port::capture]; + + // set up the camera configuration + { + MMAL_PARAMETER_CAMERA_CONFIG_T cam_config; + cam_config.hdr.id = MMAL_PARAMETER_CAMERA_CONFIG; + cam_config.hdr.size = sizeof(cam_config); + cam_config.max_stills_w = state.width; + cam_config.max_stills_h = state.height; + cam_config.stills_yuv422 = 0; + cam_config.one_shot_stills = 0; + cam_config.max_preview_video_w = state.width; + cam_config.max_preview_video_h = state.height; + cam_config.num_preview_video_frames = 3; + cam_config.stills_capture_circular_buffer_height = 0; + cam_config.fast_preview_resume = 0; + cam_config.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC; + + mmal_port_parameter_set(camera->control, &cam_config.hdr); + } + + // Select the camera to use + { + MMAL_PARAMETER_INT32_T camera_num; + camera_num.hdr.id = MMAL_PARAMETER_CAMERA_NUM; + camera_num.hdr.size = sizeof(camera_num); + camera_num.value = state.camera_id; + + status = mmal_port_parameter_set(camera->control, &camera_num.hdr); + if (status != MMAL_SUCCESS) { + ROS_ERROR("Could not select camera : error %d", status); + goto error; + } + } + + // Now set up the port formats + + // Set the encode format on the video port + + format = video_port->format; + format->encoding_variant = MMAL_ENCODING_I420; + + format->encoding = MMAL_ENCODING_I420; + format->es->video.width = state.width; + format->es->video.height = state.height; + format->es->video.crop.x = 0; + format->es->video.crop.y = 0; + format->es->video.crop.width = state.width; + format->es->video.crop.height = state.height; + format->es->video.frame_rate.num = state.framerate; + format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN; + + status = mmal_port_format_commit(video_port); + + if (status) { + vcos_log_error("camera video format couldn't be set"); + ROS_ERROR("camera video format couldn't be set"); + goto error; + } + + // Ensure there are enough buffers to avoid dropping frames + if (video_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) + video_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; + + // Set the encode format on the preview port + + format = preview_port->format; + format->encoding_variant = MMAL_ENCODING_I420; + + format->encoding = MMAL_ENCODING_I420; + format->es->video.width = state.width; + format->es->video.height = state.height; + format->es->video.crop.x = 0; + format->es->video.crop.y = 0; + format->es->video.crop.width = state.width; + format->es->video.crop.height = state.height; + format->es->video.frame_rate.num = state.framerate; + format->es->video.frame_rate.den = VIDEO_FRAME_RATE_DEN; + + status = mmal_port_format_commit(preview_port); + + if (status) { + vcos_log_error("camera preview format couldn't be set"); + ROS_ERROR("camera preview format couldn't be set"); + goto error; + } + + // Ensure there are enough buffers to avoid dropping frames + if (preview_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) + preview_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; + + // Set the encode format on the still port + + format = still_port->format; + + format->encoding = MMAL_ENCODING_OPAQUE; + format->encoding_variant = MMAL_ENCODING_I420; + + format->es->video.width = state.width; + format->es->video.height = state.height; + format->es->video.crop.x = 0; + format->es->video.crop.y = 0; + format->es->video.crop.width = state.width; + format->es->video.crop.height = state.height; + format->es->video.frame_rate.num = 1; + format->es->video.frame_rate.den = 1; + + status = mmal_port_format_commit(still_port); + + if (status) { + vcos_log_error("camera still format couldn't be set"); + ROS_ERROR("camera still format couldn't be set"); + goto error; + } + + video_port->buffer_num = video_port->buffer_num_recommended; + /* Ensure there are enough buffers to avoid dropping frames */ + if (still_port->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) + still_port->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; + + /* Enable component */ + status = mmal_component_enable(camera); + + if (status) { + vcos_log_error("camera component couldn't be enabled"); + ROS_ERROR("camera component couldn't be enabled"); + goto error; + } + + raspicamcontrol_set_all_parameters(camera, &state.camera_parameters); + + state.camera_component.reset(camera); + + ROS_DEBUG("Camera component done\n"); + + return camera; + +error: + + if (camera) + mmal_component_destroy(camera); + return 0; +} + +/** + * Create the splitter component, set up its ports + * + * @param state Pointer to state control struct + * + * @return MMAL_SUCCESS if all OK, something else otherwise + * + */ +static MMAL_STATUS_T create_splitter_component(RASPIVID_STATE& state) { + MMAL_COMPONENT_T* splitter = 0; + MMAL_PORT_T* splitter_input = nullptr; + MMAL_PORT_T *splitter_output_enc = nullptr, *splitter_output_raw = nullptr; + MMAL_STATUS_T status; + MMAL_POOL_T* pool; + MMAL_ES_FORMAT_T* format; + + status = mmal_component_create(MMAL_COMPONENT_DEFAULT_VIDEO_SPLITTER, &splitter); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Unable to create image encoder component"); + ROS_ERROR("Unable to create image encoder component"); + goto error; + } + + if (!splitter->input_num) { + status = MMAL_ENOSYS; + ROS_ERROR("Video splitter doesn't have input ports"); + goto error; + } + + if (splitter->output_num < 2) { + status = MMAL_ENOSYS; + ROS_ERROR("Video splitter doesn't have enough output ports"); + goto error; + } + + /*** Input Port setup ***/ + + splitter_input = splitter->input[0]; + + // We want same format on input as camera output + mmal_format_copy(splitter_input->format, state.camera_component->output[mmal::camera_port::video]->format); + + if (splitter->input[0]->buffer_num < VIDEO_OUTPUT_BUFFERS_NUM) + splitter->input[0]->buffer_num = VIDEO_OUTPUT_BUFFERS_NUM; + + // Commit the port changes to the output port + status = mmal_port_format_commit(splitter_input); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Unable to set format on splitter input port"); + ROS_ERROR("Unable to set format on splitter input port"); + goto error; + } + + /*** Output to image encoder setup ***/ + + splitter_output_enc = splitter->output[0]; + + // Copy the format from the splitter input + mmal_format_copy(splitter_output_enc->format, splitter_input->format); + + status = mmal_port_format_commit(splitter_output_enc); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Unable to set format on splitter output port for image encoder"); + goto error; + } + + /*** Output for raw ***/ + + splitter_output_raw = splitter->output[1]; + + // Copy the format from the splitter input + mmal_format_copy(splitter_output_raw->format, splitter_input->format); + + // Use BGR24 (bgr8 in ROS) + format = splitter_output_raw->format; + format->encoding = MMAL_ENCODING_BGR24; + format->encoding_variant = 0; /* Irrelevant when not in opaque mode */ + + status = mmal_port_format_commit(splitter_output_raw); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Unable to set format on splitter output port for raw"); + goto error; + } + + /*** Setup all other output ports ***/ + + // start from 2 + for (unsigned int i = 2; i < splitter->output_num; i++) { + mmal_format_copy(splitter->output[i]->format, splitter_input->format); + + status = mmal_port_format_commit(splitter->output[i]); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Unable to set format on splitter output port %d", i); + goto error; + } + } + + /*** Enable component ***/ + + status = mmal_component_enable(splitter); + + if (status != MMAL_SUCCESS) { + vcos_log_error("Unable to enable splitter component"); + ROS_ERROR("Unable to enable splitter component"); + goto error; + } + + /*** Create Pool ***/ + + // Create pool of buffer headers for the raw output port to consume + pool = mmal_port_pool_create(splitter_output_raw, splitter_output_raw->buffer_num, splitter_output_raw->buffer_size); + + if (!pool) { + vcos_log_error("Failed to create buffer header pool for image encoder output port %s", splitter_output_raw->name); + ROS_ERROR("Failed to create buffer header pool for image encoder output port %s", splitter_output_raw->name); + } + + /*** Push to state struct ***/ + + state.splitter_pool = mmal::pool_ptr(pool, [splitter](MMAL_POOL_T* ptr) { + if (splitter->output[1] && splitter->output[1]->is_enabled) { + mmal_port_disable(splitter->output[1]); + } + mmal_port_pool_destroy(splitter->output[1], ptr); + }); + + state.splitter_component.reset(splitter); + + ROS_INFO("splitter component done\n"); + + return status; + +error: + if (splitter) + mmal_component_destroy(splitter); + + return status; +} + +/** + * Connect two specific ports together + * + * @param output_port Pointer the output port + * @param input_port Pointer the input port + * @param Pointer to a mmal connection pointer, reassigned if function + * successful + * @return Returns a MMAL_STATUS_T giving result of operation + * + */ +static MMAL_STATUS_T connect_ports(MMAL_PORT_T* output_port, MMAL_PORT_T* input_port, + mmal::connection_ptr& connection) { + MMAL_STATUS_T status; + + MMAL_CONNECTION_T* new_connection = nullptr; + + status = mmal_connection_create(&new_connection, output_port, input_port, + MMAL_CONNECTION_FLAG_TUNNELLING | MMAL_CONNECTION_FLAG_ALLOCATION_ON_INPUT); + + if (status == MMAL_SUCCESS) { + status = mmal_connection_enable(new_connection); + if (status != MMAL_SUCCESS) + mmal_connection_destroy(new_connection); + } + + connection.reset(new_connection); + + return status; +} + +/** + * init_cam + + */ +int init_cam(RASPIVID_STATE& state) { + MMAL_STATUS_T status; + MMAL_PORT_T* camera_video_port = nullptr; + MMAL_PORT_T* splitter_input_port = nullptr; + MMAL_PORT_T* splitter_output_raw = nullptr; + + bcm_host_init(); + // Register our application with the logging system + vcos_log_register("RaspiVid", VCOS_LOG_CATEGORY); + + // OK, we have a nice set of parameters. Now set up our components + // We have three components. Camera, splitter and encoder. + + if (!create_camera_component(state)) { + ROS_ERROR("%s: Failed to create camera component", __func__); + } else if ((status = create_splitter_component(state)) != MMAL_SUCCESS) { + ROS_ERROR("%s: Failed to create splitter component", __func__); + // state.image_encoder_component.reset(nullptr); + // state.video_encoder_component.reset(nullptr); + state.camera_component.reset(nullptr); + } else { + camera_video_port = state.camera_component->output[mmal::camera_port::video]; + splitter_input_port = state.splitter_component->input[0]; + + status = connect_ports(camera_video_port, splitter_input_port, state.splitter_connection); + if (status != MMAL_SUCCESS) { + ROS_ERROR("%s: Failed to connect camera video port to splitter input", __func__); + return 1; + } + splitter_output_raw = state.splitter_component->output[1]; + + PORT_USERDATA* callback_data_raw = new PORT_USERDATA(state); + callback_data_raw->buffer[0] = std::make_unique(IMG_BUFFER_SIZE); + callback_data_raw->buffer[1] = std::make_unique(IMG_BUFFER_SIZE); + // Set up our userdata - this is passed though to the callback where we + // need the information. + callback_data_raw->abort = false; + callback_data_raw->id = 0; + callback_data_raw->frame = 0; + splitter_output_raw->userdata = callback_data_raw; + // Enable the splitter output port and tell it its callback function + status = mmal_port_enable(splitter_output_raw, splitter_buffer_callback); + if (status != MMAL_SUCCESS) { + ROS_ERROR("Failed to setup splitter output"); + return 1; + } + state.isInit = true; + } + return 0; +} + +int start_capture(RASPIVID_STATE& state) { + if (!(state.isInit)) + ROS_FATAL("Tried to start capture before camera is inited"); + + MMAL_PORT_T* camera_video_port = state.camera_component->output[mmal::camera_port::video]; + MMAL_PORT_T* splitter_output_raw = state.splitter_component->output[1]; + ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state.width, state.height/2, state.quality, state.framerate); + + if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) { + return 1; + } + int num = mmal_queue_length(state.splitter_pool->queue); + int q; + for (q = 0; q < num; q++) { + MMAL_BUFFER_HEADER_T* buffer = mmal_queue_get(state.splitter_pool->queue); + + if (!buffer) { + vcos_log_error("Unable to get a required buffer %d from pool queue", q); + ROS_ERROR("Unable to get a required buffer %d from pool queue", q); + } + + if (mmal_port_send_buffer(splitter_output_raw, buffer) != MMAL_SUCCESS) { + vcos_log_error("Unable to send a buffer to splitter output port (%d)", q); + ROS_ERROR("Unable to send a buffer to splitter output port (%d)", q); + } + } + ROS_INFO("Video capture started\n"); + return 0; +} + +int close_cam(RASPIVID_STATE& state) { + if (state.isInit) { + state.isInit = false; + MMAL_COMPONENT_T* camera = state.camera_component.get(); + MMAL_COMPONENT_T* splitter = state.splitter_component.get(); + + // Destroy splitter port connection + state.splitter_connection.reset(nullptr); + + + // Destroy splitter component + if (splitter) { + // Get rid of any port buffers first + state.splitter_pool.reset(nullptr); + // Delete callback structure + if (splitter->output[1]->userdata) { + delete splitter->output[1]->userdata; + } + state.splitter_component.reset(nullptr); + } + + // destroy camera component + if (camera) { + state.camera_component.reset(nullptr); + } + ROS_INFO("Video capture stopped\n"); + return 0; + } else + return 1; +} + +void reconfigure_callback(raspicam_node::CameraConfig& config, uint32_t level, RASPIVID_STATE& state) { + ROS_DEBUG("figure Request: contrast %d, sharpness %d, brightness %d, " + "saturation %d, ISO %d, exposureCompensation %d," + " videoStabilisation %d, vFlip %d, hFlip %d," + " zoom %.2f, exposure_mode %s, awb_mode %s, shutter_speed %d", + config.contrast, config.sharpness, config.brightness, config.saturation, config.ISO, + config.exposure_compensation, config.video_stabilisation, config.vFlip, config.hFlip, config.zoom, + config.exposure_mode.c_str(), config.awb_mode.c_str(), config.shutter_speed); + + if (!state.camera_component.get()) { + ROS_WARN("reconfiguring, but camera_component not initialized"); + return; + } + + if (config.zoom < 1.0) { + ROS_ERROR("Zoom value %f too small (must be at least 1.0)", config.zoom); + } else { + const double size = 1.0 / config.zoom; + const double offset = (1.0 - size) / 2.0; + PARAM_FLOAT_RECT_T roi; + roi.x = roi.y = offset; + roi.w = roi.h = size; + raspicamcontrol_set_ROI(state.camera_component.get(), roi); + } + + raspicamcontrol_set_exposure_mode(state.camera_component.get(), exposure_mode_from_string(config.exposure_mode.c_str())); + + raspicamcontrol_set_awb_mode(state.camera_component.get(), awb_mode_from_string(config.awb_mode.c_str())); + + raspicamcontrol_set_contrast(state.camera_component.get(), config.contrast); + raspicamcontrol_set_sharpness(state.camera_component.get(), config.sharpness); + raspicamcontrol_set_brightness(state.camera_component.get(), config.brightness); + raspicamcontrol_set_saturation(state.camera_component.get(), config.saturation); + raspicamcontrol_set_ISO(state.camera_component.get(), config.ISO); + raspicamcontrol_set_exposure_compensation(state.camera_component.get(), config.exposure_compensation); + raspicamcontrol_set_video_stabilisation(state.camera_component.get(), config.video_stabilisation); + raspicamcontrol_set_flips(state.camera_component.get(), config.hFlip, config.vFlip); + raspicamcontrol_set_shutter_speed(state.camera_component.get(), config.shutter_speed); + + ROS_DEBUG("Reconfigure done"); +} + +int main(int argc, char** argv) { +#ifdef _DEBUG + DBG_MSG("//DEBUG// - Init rosnode!!!") +#endif // _DEBUG + ros::init(argc, argv, "stereo_node"); + ros::NodeHandle nh_params("~"); +#ifdef _DEBUG + DBG_MSG("//DEBUG// - Read parameters!!!") +#endif // _DEBUG + bool private_topics; + nh_params.param("private_topics", private_topics, true); + + // The node handle used for topics will be private or public depending on the value of the ~private_topics parameter + ros::NodeHandle nh_topics(private_topics ? std::string("~") : std::string()); + + nh_params.param("skip_frames", skip_frames, 0); + + std::string left_camera_info_url, right_camera_info_url; + std::string left_camera_name, right_camera_name; + + nh_params.param("left_camera_info_url", left_camera_info_url, std::string("package://raspicam_node/camera_info/left_camera.yaml")); + nh_params.param("right_camera_info_url", right_camera_info_url, std::string("package://raspicam_node/camera_info/right_camera.yaml")); + nh_params.param("left_camera_name", left_camera_name, std::string("left_camera")); + nh_params.param("right_camera_name", right_camera_name, std::string("right_camera")); + + camera_info_manager::CameraInfoManager left_info_man(nh_params, left_camera_name, left_camera_info_url); + // camera_info_manager::CameraInfoManager right_info_man(nh_params, right_camera_name, right_camera_info_url); + + RASPIVID_STATE state_srv; +#ifdef _DEBUG + DBG_MSG("//DEBUG// - configure_parameters!!!") +#endif // _DEBUG + configure_parameters(state_srv, nh_params); +#ifdef _DEBUG + DBG_MSG("//DEBUG// - parameters were set!!!") + DBG_MSG("//DEBUG// - Init camera!!!") +#endif // _DEBUG + init_cam(state_srv); +#ifdef _DEBUG + DBG_MSG("//DEBUG// - Done!!!") + DBG_MSG("//DEBUG// - Load left camera calibration!!!") +#endif // _DEBUG + if(!left_info_man.loadCameraInfo(left_camera_info_url)) { + ROS_INFO("Calibration file missing. Left camera not calibrated"); + } else { + left_info = left_info_man.getCameraInfo(); + ROS_INFO("Camera successfully calibrated from config file"); + } +#ifdef _DEBUG + DBG_MSG("//DEBUG// - Load right camera calibration!!!") +#endif // _DEBUG + if (!left_info_man.loadCameraInfo(right_camera_info_url)) { + ROS_INFO("Calibration file missing. Right camera not calibrated"); + } else { + right_info = left_info_man.getCameraInfo(); + ROS_INFO("Camera successfully calibrated from config file"); + } + +#ifdef _DEBUG + DBG_MSG("setHardwareID!!!") +#endif // _DEBUG + + // diagnostics parameters + state_srv.updater.setHardwareID("raspicam"); + double desired_freq = state_srv.framerate; + double min_freq = desired_freq * 0.95; + double max_freq = desired_freq * 1.05; +#ifdef _DEBUG + DBG_MSG("//DEBUG// - Advertizing!!!") +#endif // _DEBUG + auto left_image_pub = nh_topics.advertise("left_image", 2); + left_image.pub.reset(new DiagnosedPublisher( + left_image_pub, state_srv.updater, FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10), TimeStampStatusParam(0, 0.2))); // if (state_srv.enable_imv_pub) { + auto right_image_pub = nh_topics.advertise("right_image", 2); + right_image.pub.reset(new DiagnosedPublisher( + right_image_pub, state_srv.updater, FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10), TimeStampStatusParam(0, 0.2))); + if(left_image.pub == nullptr || right_image.pub == nullptr) + { + ROS_INFO("Publisher is corrupted"); + return 0; + } + left_camera_info_pub = nh_topics.advertise("left_camera_info", 2); + right_camera_info_pub = nh_topics.advertise("right_camera_info", 2); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&reconfigure_callback, _1, _2, boost::ref(state_srv)); +#ifdef _DEBUG + DBG_MSG("//DEBUG// - Setting up Dynamic reconfigure!!!") +#endif // _DEBUG + server.setCallback(f); +#ifdef _DEBUG + DBG_MSG("//DEBUG// - start_capture!!!") +#endif // _DEBUG + start_capture(state_srv); + ros::spin(); + close_cam(state_srv); + ros::shutdown(); +} + +#endif // __arm__ From e3cb34627da32937e69e57e8289756d0d3cebb17 Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:40:48 +0400 Subject: [PATCH 02/18] for launching stereo_node --- launch/stereo_1280x720.launch | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 launch/stereo_1280x720.launch diff --git a/launch/stereo_1280x720.launch b/launch/stereo_1280x720.launch new file mode 100644 index 0000000..585ce3a --- /dev/null +++ b/launch/stereo_1280x720.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + From 9fe247c263f7d1be628e211958b3dbe860a2355c Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:41:31 +0400 Subject: [PATCH 03/18] mmal library api was updated --- include/RaspiCLI.h | 56 ++ include/RaspiCamControl.h | 409 ++++----- src/RaspiCLI.c | 155 ++++ src/RaspiCamControl.c | 1645 +++++++++++++++++++++++++++++++++++++ 4 files changed, 2081 insertions(+), 184 deletions(-) create mode 100644 include/RaspiCLI.h create mode 100644 src/RaspiCLI.c create mode 100755 src/RaspiCamControl.c diff --git a/include/RaspiCLI.h b/include/RaspiCLI.h new file mode 100644 index 0000000..26b1ce3 --- /dev/null +++ b/include/RaspiCLI.h @@ -0,0 +1,56 @@ +/* +Copyright (c) 2013, Broadcom Europe Ltd +Copyright (c) 2013, James Hughes +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef RASPICLI_H_ +#define RASPICLI_H_ + +typedef struct +{ + int id; + char *command; + char *abbrev; + char *help; + int num_parameters; +} COMMAND_LIST; + +/// Cross reference structure, mode string against mode id +typedef struct xref_t +{ + char *mode; + int mmal_mode; +} XREF_T; + + +void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands); +int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters); + +int raspicli_map_xref(const char *str, const XREF_T *map, int num_refs); +const char *raspicli_unmap_xref(const int en, XREF_T *map, int num_refs); + + +#endif diff --git a/include/RaspiCamControl.h b/include/RaspiCamControl.h index 7b6543b..55cbc63 100644 --- a/include/RaspiCamControl.h +++ b/include/RaspiCamControl.h @@ -1,184 +1,225 @@ -/* -Copyright (c) 2013, Broadcom Europe Ltd -Copyright (c) 2013, James Hughes -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the copyright holder nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifndef RASPICAMCONTROL_H_ -#define RASPICAMCONTROL_H_ - -#include "interface/mmal/mmal.h" -#include "interface/mmal/mmal_logging.h" -#include "interface/mmal/util/mmal_default_components.h" -#include "interface/mmal/util/mmal_util.h" -#include "interface/mmal/util/mmal_util_params.h" -#include "interface/vmcs_host/vc_vchi_gencmd.h" - -/* Various parameters - * - * Exposure Mode - * MMAL_PARAM_EXPOSUREMODE_OFF, - MMAL_PARAM_EXPOSUREMODE_AUTO, - MMAL_PARAM_EXPOSUREMODE_NIGHT, - MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, - MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, - MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, - MMAL_PARAM_EXPOSUREMODE_SPORTS, - MMAL_PARAM_EXPOSUREMODE_SNOW, - MMAL_PARAM_EXPOSUREMODE_BEACH, - MMAL_PARAM_EXPOSUREMODE_VERYLONG, - MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, - MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, - MMAL_PARAM_EXPOSUREMODE_FIREWORKS, - * - * AWB Mode - * MMAL_PARAM_AWBMODE_OFF, - MMAL_PARAM_AWBMODE_AUTO, - MMAL_PARAM_AWBMODE_SUNLIGHT, - MMAL_PARAM_AWBMODE_CLOUDY, - MMAL_PARAM_AWBMODE_SHADE, - MMAL_PARAM_AWBMODE_TUNGSTEN, - MMAL_PARAM_AWBMODE_FLUORESCENT, - MMAL_PARAM_AWBMODE_INCANDESCENT, - MMAL_PARAM_AWBMODE_FLASH, - MMAL_PARAM_AWBMODE_HORIZON, - * - * Image FX - MMAL_PARAM_IMAGEFX_NONE, - MMAL_PARAM_IMAGEFX_NEGATIVE, - MMAL_PARAM_IMAGEFX_SOLARIZE, - MMAL_PARAM_IMAGEFX_POSTERIZE, - MMAL_PARAM_IMAGEFX_WHITEBOARD, - MMAL_PARAM_IMAGEFX_BLACKBOARD, - MMAL_PARAM_IMAGEFX_SKETCH, - MMAL_PARAM_IMAGEFX_DENOISE, - MMAL_PARAM_IMAGEFX_EMBOSS, - MMAL_PARAM_IMAGEFX_OILPAINT, - MMAL_PARAM_IMAGEFX_HATCH, - MMAL_PARAM_IMAGEFX_GPEN, - MMAL_PARAM_IMAGEFX_PASTEL, - MMAL_PARAM_IMAGEFX_WATERCOLOUR, - MMAL_PARAM_IMAGEFX_FILM, - MMAL_PARAM_IMAGEFX_BLUR, - MMAL_PARAM_IMAGEFX_SATURATION, - MMAL_PARAM_IMAGEFX_COLOURSWAP, - MMAL_PARAM_IMAGEFX_WASHEDOUT, - MMAL_PARAM_IMAGEFX_POSTERISE, - MMAL_PARAM_IMAGEFX_COLOURPOINT, - MMAL_PARAM_IMAGEFX_COLOURBALANCE, - MMAL_PARAM_IMAGEFX_CARTOON, - - */ - -// There isn't actually a MMAL structure for the following, so make one -typedef struct { - int enable; /// Turn colourFX on or off - int u, v; /// U and V to use -} MMAL_PARAM_COLOURFX_T; - -typedef struct { - int enable; - int width, height; - int quality; -} MMAL_PARAM_THUMBNAIL_CONFIG_T; - -typedef struct { - double x; - double y; - double w; - double h; -} PARAM_FLOAT_RECT_T; - -/// struct contain camera settings -typedef struct { - int sharpness; /// -100 to 100 - int contrast; /// -100 to 100 - int brightness; /// 0 to 100 - int saturation; /// -100 to 100 - int ISO; /// TODO : what range? - int videoStabilisation; /// 0 or 1 (false or true) - int exposureCompensation; /// -10 to +10 ? - MMAL_PARAM_EXPOSUREMODE_T exposureMode; - MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode; - MMAL_PARAM_AWBMODE_T awbMode; - MMAL_PARAM_IMAGEFX_T imageEffect; - MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters; - MMAL_PARAM_COLOURFX_T colourEffects; - int rotation; /// 0-359 - int hflip; /// 0 or 1 - int vflip; /// 0 or 1 - PARAM_FLOAT_RECT_T roi; /// region of interest to use on the sensor. - /// Normalised [0,1] values in the rect - int shutter_speed; /// TODO : Range? -} RASPICAM_CAMERA_PARAMETERS; - -MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char* str); -MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char* str); - -void raspicamcontrol_check_configuration(int min_gpu_mem); - -int raspicamcontrol_cycle_test(MMAL_COMPONENT_T& camera); - -int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T& camera, const RASPICAM_CAMERA_PARAMETERS& params); -void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS& params); - -void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS& params); - -void raspicamcontrol_check_configuration(int min_gpu_mem); - -// Individual setting functions -int raspicamcontrol_set_saturation(MMAL_COMPONENT_T& camera, int saturation); -int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T& camera, int sharpness); -int raspicamcontrol_set_contrast(MMAL_COMPONENT_T& camera, int contrast); -int raspicamcontrol_set_brightness(MMAL_COMPONENT_T& camera, int brightness); -int raspicamcontrol_set_ISO(MMAL_COMPONENT_T& camera, int ISO); -int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T mode); -int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T& camera, int vstabilisation); -int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T& camera, int exp_comp); -int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMODE_T mode); -int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_AWBMODE_T awb_mode); -int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T& camera, MMAL_PARAM_IMAGEFX_T imageFX); -int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T& camera, const MMAL_PARAM_COLOURFX_T* colourFX); -int raspicamcontrol_set_rotation(MMAL_COMPONENT_T& camera, int rotation); -int raspicamcontrol_set_flips(MMAL_COMPONENT_T& camera, int hflip, int vflip); -int raspicamcontrol_set_ROI(MMAL_COMPONENT_T& camera, PARAM_FLOAT_RECT_T rect); -int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T& camera, int shutter_speed); - -// Individual getting functions -int raspicamcontrol_get_saturation(MMAL_COMPONENT_T& camera); -int raspicamcontrol_get_sharpness(MMAL_COMPONENT_T& camera); -int raspicamcontrol_get_contrast(MMAL_COMPONENT_T& camera); -int raspicamcontrol_get_brightness(MMAL_COMPONENT_T& camera); -int raspicamcontrol_get_ISO(MMAL_COMPONENT_T& camera); -MMAL_PARAM_EXPOSUREMETERINGMODE_T raspicamcontrol_get_metering_mode(MMAL_COMPONENT_T& camera); -int raspicamcontrol_get_video_stabilisation(MMAL_COMPONENT_T& camera); -int raspicamcontrol_get_exposure_compensation(MMAL_COMPONENT_T& camera); -MMAL_PARAM_THUMBNAIL_CONFIG_T raspicamcontrol_get_thumbnail_parameters(MMAL_COMPONENT_T& camera); -MMAL_PARAM_EXPOSUREMODE_T raspicamcontrol_get_exposure_mode(MMAL_COMPONENT_T& camera); -MMAL_PARAM_AWBMODE_T raspicamcontrol_get_awb_mode(MMAL_COMPONENT_T& camera); -MMAL_PARAM_IMAGEFX_T raspicamcontrol_get_imageFX(MMAL_COMPONENT_T& camera); -MMAL_PARAM_COLOURFX_T raspicamcontrol_get_colourFX(MMAL_COMPONENT_T& camera); - -#endif /* RASPICAMCONTROL_H_ */ +/* +Copyright (c) 2013, Broadcom Europe Ltd +Copyright (c) 2013, James Hughes +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#ifndef RASPICAMCONTROL_H_ +#define RASPICAMCONTROL_H_ + +/* Various parameters + * + * Exposure Mode + * MMAL_PARAM_EXPOSUREMODE_OFF, + MMAL_PARAM_EXPOSUREMODE_AUTO, + MMAL_PARAM_EXPOSUREMODE_NIGHT, + MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, + MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, + MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, + MMAL_PARAM_EXPOSUREMODE_SPORTS, + MMAL_PARAM_EXPOSUREMODE_SNOW, + MMAL_PARAM_EXPOSUREMODE_BEACH, + MMAL_PARAM_EXPOSUREMODE_VERYLONG, + MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, + MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, + MMAL_PARAM_EXPOSUREMODE_FIREWORKS, + * + * AWB Mode + * MMAL_PARAM_AWBMODE_OFF, + MMAL_PARAM_AWBMODE_AUTO, + MMAL_PARAM_AWBMODE_SUNLIGHT, + MMAL_PARAM_AWBMODE_CLOUDY, + MMAL_PARAM_AWBMODE_SHADE, + MMAL_PARAM_AWBMODE_TUNGSTEN, + MMAL_PARAM_AWBMODE_FLUORESCENT, + MMAL_PARAM_AWBMODE_INCANDESCENT, + MMAL_PARAM_AWBMODE_FLASH, + MMAL_PARAM_AWBMODE_HORIZON, + * + * Image FX + MMAL_PARAM_IMAGEFX_NONE, + MMAL_PARAM_IMAGEFX_NEGATIVE, + MMAL_PARAM_IMAGEFX_SOLARIZE, + MMAL_PARAM_IMAGEFX_POSTERIZE, + MMAL_PARAM_IMAGEFX_WHITEBOARD, + MMAL_PARAM_IMAGEFX_BLACKBOARD, + MMAL_PARAM_IMAGEFX_SKETCH, + MMAL_PARAM_IMAGEFX_DENOISE, + MMAL_PARAM_IMAGEFX_EMBOSS, + MMAL_PARAM_IMAGEFX_OILPAINT, + MMAL_PARAM_IMAGEFX_HATCH, + MMAL_PARAM_IMAGEFX_GPEN, + MMAL_PARAM_IMAGEFX_PASTEL, + MMAL_PARAM_IMAGEFX_WATERCOLOUR, + MMAL_PARAM_IMAGEFX_FILM, + MMAL_PARAM_IMAGEFX_BLUR, + MMAL_PARAM_IMAGEFX_SATURATION, + MMAL_PARAM_IMAGEFX_COLOURSWAP, + MMAL_PARAM_IMAGEFX_WASHEDOUT, + MMAL_PARAM_IMAGEFX_POSTERISE, + MMAL_PARAM_IMAGEFX_COLOURPOINT, + MMAL_PARAM_IMAGEFX_COLOURBALANCE, + MMAL_PARAM_IMAGEFX_CARTOON, + + */ + +/// Annotate bitmask options +/// Supplied by user on command line +#define ANNOTATE_USER_TEXT 1 +/// Supplied by app using this module +#define ANNOTATE_APP_TEXT 2 +/// Insert current date +#define ANNOTATE_DATE_TEXT 4 +// Insert current time +#define ANNOTATE_TIME_TEXT 8 + +#define ANNOTATE_SHUTTER_SETTINGS 16 +#define ANNOTATE_CAF_SETTINGS 32 +#define ANNOTATE_GAIN_SETTINGS 64 +#define ANNOTATE_LENS_SETTINGS 128 +#define ANNOTATE_MOTION_SETTINGS 256 +#define ANNOTATE_FRAME_NUMBER 512 +#define ANNOTATE_BLACK_BACKGROUND 1024 + + +// There isn't actually a MMAL structure for the following, so make one +typedef struct mmal_param_colourfx_s +{ + int enable; /// Turn colourFX on or off + int u,v; /// U and V to use +} MMAL_PARAM_COLOURFX_T; + +typedef struct mmal_param_thumbnail_config_s +{ + int enable; + int width,height; + int quality; +} MMAL_PARAM_THUMBNAIL_CONFIG_T; + +typedef struct param_float_rect_s +{ + double x; + double y; + double w; + double h; +} PARAM_FLOAT_RECT_T; + +/// struct contain camera settings +typedef struct raspicam_camera_parameters_s +{ + int sharpness; /// -100 to 100 + int contrast; /// -100 to 100 + int brightness; /// 0 to 100 + int saturation; /// -100 to 100 + int ISO; /// TODO : what range? + int videoStabilisation; /// 0 or 1 (false or true) + int exposureCompensation; /// -10 to +10 ? + MMAL_PARAM_EXPOSUREMODE_T exposureMode; + MMAL_PARAM_EXPOSUREMETERINGMODE_T exposureMeterMode; + MMAL_PARAM_AWBMODE_T awbMode; + MMAL_PARAM_IMAGEFX_T imageEffect; + MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imageEffectsParameters; + MMAL_PARAM_COLOURFX_T colourEffects; + int rotation; /// 0-359 + int hflip; /// 0 or 1 + int vflip; /// 0 or 1 + PARAM_FLOAT_RECT_T roi; /// region of interest to use on the sensor. Normalised [0,1] values in the rect + int shutter_speed; /// 0 = auto, otherwise the shutter speed in ms + float awb_gains_r; /// AWB red gain + float awb_gains_b; /// AWB blue gain + MMAL_PARAMETER_DRC_STRENGTH_T drc_level; // Strength of Dynamic Range compression to apply + MMAL_BOOL_T stats_pass; /// Stills capture statistics pass on/off + int enable_annotate; /// Flag to enable the annotate, 0 = disabled, otherwise a bitmask of what needs to be displayed + char annotate_string[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V2]; /// String to use for annotate - overrides certain bitmask settings + int annotate_text_size; // Text size for annotation + int annotate_text_colour; // Text colour for annotation + int annotate_bg_colour; // Background colour for annotation + MMAL_PARAMETER_STEREOSCOPIC_MODE_T stereo_mode; +} RASPICAM_CAMERA_PARAMETERS; + +typedef enum { + ZOOM_IN, ZOOM_OUT, ZOOM_RESET +} ZOOM_COMMAND_T; + +MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char *str); +MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char *str); +MMAL_STEREOSCOPIC_MODE_T stereo_mode_from_string(const char *str); +void raspicamcontrol_check_configuration(int min_gpu_mem); + +int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2); +void raspicamcontrol_display_help(); +int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera); + +int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params); +int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params); +void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS *params); + +void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params); + +void raspicamcontrol_check_configuration(int min_gpu_mem); + +// Individual setting functions +int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation); +int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness); +int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast); +int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness); +int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO); +int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T mode); +int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation); +int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp); +int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode); +int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode); +int raspicamcontrol_set_awb_gains(MMAL_COMPONENT_T *camera, float r_gain, float b_gain); +int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX); +int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX); +int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation); +int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip); +int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect); +int raspicamcontrol_zoom_in_zoom_out(MMAL_COMPONENT_T *camera, ZOOM_COMMAND_T zoom_command, PARAM_FLOAT_RECT_T *roi); +int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T *camera, int speed_ms); +int raspicamcontrol_set_DRC(MMAL_COMPONENT_T *camera, MMAL_PARAMETER_DRC_STRENGTH_T strength); +int raspicamcontrol_set_stats_pass(MMAL_COMPONENT_T *camera, int stats_pass); +int raspicamcontrol_set_annotate(MMAL_COMPONENT_T *camera, const int bitmask, const char *string, + const int text_size, const int text_colour, const int bg_colour); +int raspicamcontrol_set_stereo_mode(MMAL_PORT_T *port, MMAL_PARAMETER_STEREOSCOPIC_MODE_T *stereo_mode); + +//Individual getting functions +int raspicamcontrol_get_saturation(MMAL_COMPONENT_T *camera); +int raspicamcontrol_get_sharpness(MMAL_COMPONENT_T *camera); +int raspicamcontrol_get_contrast(MMAL_COMPONENT_T *camera); +int raspicamcontrol_get_brightness(MMAL_COMPONENT_T *camera); +int raspicamcontrol_get_ISO(MMAL_COMPONENT_T *camera); +MMAL_PARAM_EXPOSUREMETERINGMODE_T raspicamcontrol_get_metering_mode(MMAL_COMPONENT_T *camera); +int raspicamcontrol_get_video_stabilisation(MMAL_COMPONENT_T *camera); +int raspicamcontrol_get_exposure_compensation(MMAL_COMPONENT_T *camera); +MMAL_PARAM_THUMBNAIL_CONFIG_T raspicamcontrol_get_thumbnail_parameters(MMAL_COMPONENT_T *camera); +MMAL_PARAM_EXPOSUREMODE_T raspicamcontrol_get_exposure_mode(MMAL_COMPONENT_T *camera); +MMAL_PARAM_AWBMODE_T raspicamcontrol_get_awb_mode(MMAL_COMPONENT_T *camera); +MMAL_PARAM_IMAGEFX_T raspicamcontrol_get_imageFX(MMAL_COMPONENT_T *camera); +MMAL_PARAM_COLOURFX_T raspicamcontrol_get_colourFX(MMAL_COMPONENT_T *camera); + + + +#endif /* RASPICAMCONTROL_H_ */ diff --git a/src/RaspiCLI.c b/src/RaspiCLI.c new file mode 100644 index 0000000..9f858f4 --- /dev/null +++ b/src/RaspiCLI.c @@ -0,0 +1,155 @@ +/* +Copyright (c) 2013, Broadcom Europe Ltd +Copyright (c) 2013, James Hughes +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the copyright holder nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +/** + * \file RaspiCLI.c + * Code for handling command line parameters + * + * \date 4th March 2013 + * \Author: James Hughes + * + * Description + * + * Some functions/structures for command line parameter parsing + * + */ +#include +#include +#include +#include + +#include "interface/vcos/vcos.h" + +#include "RaspiCLI.h" + + +/** + * Convert a string from command line to a comand_id from the list + * + * @param commands Array of command to check + * @param num_command Number of commands in the array + * @param arg String to search for in the list + * @param num_parameters Returns the number of parameters used by the command + * + * @return command ID if found, -1 if not found + * + */ +int raspicli_get_command_id(const COMMAND_LIST *commands, const int num_commands, const char *arg, int *num_parameters) +{ + int command_id = -1; + int j; + + vcos_assert(commands); + vcos_assert(num_parameters); + vcos_assert(arg); + + if (!commands || !num_parameters || !arg) + return -1; + + for (j = 0; j < num_commands; j++) + { + if (!strcmp(arg, commands[j].command) || + !strcmp(arg, commands[j].abbrev)) + { + // match + command_id = commands[j].id; + *num_parameters = commands[j].num_parameters; + break; + } + } + + return command_id; +} + + +/** + * Display the list of commands in help format + * + * @param commands Array of command to check + * @param num_command Number of commands in the arry + * + * + */ +void raspicli_display_help(const COMMAND_LIST *commands, const int num_commands) +{ + int i; + + vcos_assert(commands); + + if (!commands) + return; + + for (i = 0; i < num_commands; i++) + { + fprintf(stdout, "-%s, -%s\t: %s\n", commands[i].abbrev, + commands[i].command, commands[i].help); + } +} + + +/** + * Function to take a string, a mapping, and return the int equivalent + * @param str Incoming string to match + * @param map Mapping data + * @param num_refs The number of items in the mapping data + * @return The integer match for the string, or -1 if no match + */ +int raspicli_map_xref(const char *str, const XREF_T *map, int num_refs) +{ + int i; + + for (i=0;i +#include +#include + +#include "interface/vcos/vcos.h" + +#include "interface/vmcs_host/vc_vchi_gencmd.h" +#include "interface/mmal/mmal.h" +#include "interface/mmal/mmal_logging.h" +#include "interface/mmal/util/mmal_util.h" +#include "interface/mmal/util/mmal_util_params.h" +#include "interface/mmal/util/mmal_default_components.h" +#include "RaspiCamControl.h" +#include "RaspiCLI.h" + +/// Structure to cross reference exposure strings against the MMAL parameter equivalent +static XREF_T exposure_map[] = +{ + {"off", MMAL_PARAM_EXPOSUREMODE_OFF}, + {"auto", MMAL_PARAM_EXPOSUREMODE_AUTO}, + {"night", MMAL_PARAM_EXPOSUREMODE_NIGHT}, + {"nightpreview", MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW}, + {"backlight", MMAL_PARAM_EXPOSUREMODE_BACKLIGHT}, + {"spotlight", MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT}, + {"sports", MMAL_PARAM_EXPOSUREMODE_SPORTS}, + {"snow", MMAL_PARAM_EXPOSUREMODE_SNOW}, + {"beach", MMAL_PARAM_EXPOSUREMODE_BEACH}, + {"verylong", MMAL_PARAM_EXPOSUREMODE_VERYLONG}, + {"fixedfps", MMAL_PARAM_EXPOSUREMODE_FIXEDFPS}, + {"antishake", MMAL_PARAM_EXPOSUREMODE_ANTISHAKE}, + {"fireworks", MMAL_PARAM_EXPOSUREMODE_FIREWORKS} +}; + +static const int exposure_map_size = sizeof(exposure_map) / sizeof(exposure_map[0]); + +/// Structure to cross reference awb strings against the MMAL parameter equivalent +static XREF_T awb_map[] = +{ + {"off", MMAL_PARAM_AWBMODE_OFF}, + {"auto", MMAL_PARAM_AWBMODE_AUTO}, + {"sun", MMAL_PARAM_AWBMODE_SUNLIGHT}, + {"cloud", MMAL_PARAM_AWBMODE_CLOUDY}, + {"shade", MMAL_PARAM_AWBMODE_SHADE}, + {"tungsten", MMAL_PARAM_AWBMODE_TUNGSTEN}, + {"fluorescent", MMAL_PARAM_AWBMODE_FLUORESCENT}, + {"incandescent", MMAL_PARAM_AWBMODE_INCANDESCENT}, + {"flash", MMAL_PARAM_AWBMODE_FLASH}, + {"horizon", MMAL_PARAM_AWBMODE_HORIZON} +}; + +static const int awb_map_size = sizeof(awb_map) / sizeof(awb_map[0]); + +/// Structure to cross reference image effect against the MMAL parameter equivalent +static XREF_T imagefx_map[] = +{ + {"none", MMAL_PARAM_IMAGEFX_NONE}, + {"negative", MMAL_PARAM_IMAGEFX_NEGATIVE}, + {"solarise", MMAL_PARAM_IMAGEFX_SOLARIZE}, + {"sketch", MMAL_PARAM_IMAGEFX_SKETCH}, + {"denoise", MMAL_PARAM_IMAGEFX_DENOISE}, + {"emboss", MMAL_PARAM_IMAGEFX_EMBOSS}, + {"oilpaint", MMAL_PARAM_IMAGEFX_OILPAINT}, + {"hatch", MMAL_PARAM_IMAGEFX_HATCH}, + {"gpen", MMAL_PARAM_IMAGEFX_GPEN}, + {"pastel", MMAL_PARAM_IMAGEFX_PASTEL}, + {"watercolour", MMAL_PARAM_IMAGEFX_WATERCOLOUR}, + {"film", MMAL_PARAM_IMAGEFX_FILM}, + {"blur", MMAL_PARAM_IMAGEFX_BLUR}, + {"saturation", MMAL_PARAM_IMAGEFX_SATURATION}, + {"colourswap", MMAL_PARAM_IMAGEFX_COLOURSWAP}, + {"washedout", MMAL_PARAM_IMAGEFX_WASHEDOUT}, + {"posterise", MMAL_PARAM_IMAGEFX_POSTERISE}, + {"colourpoint", MMAL_PARAM_IMAGEFX_COLOURPOINT}, + {"colourbalance", MMAL_PARAM_IMAGEFX_COLOURBALANCE}, + {"cartoon", MMAL_PARAM_IMAGEFX_CARTOON} + }; + +static const int imagefx_map_size = sizeof(imagefx_map) / sizeof(imagefx_map[0]); + +static XREF_T metering_mode_map[] = +{ + {"average", MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE}, + {"spot", MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT}, + {"backlit", MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT}, + {"matrix", MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX} +}; + +static const int metering_mode_map_size = sizeof(metering_mode_map)/sizeof(metering_mode_map[0]); + +static XREF_T drc_mode_map[] = +{ + {"off", MMAL_PARAMETER_DRC_STRENGTH_OFF}, + {"low", MMAL_PARAMETER_DRC_STRENGTH_LOW}, + {"med", MMAL_PARAMETER_DRC_STRENGTH_MEDIUM}, + {"high", MMAL_PARAMETER_DRC_STRENGTH_HIGH} +}; + +static const int drc_mode_map_size = sizeof(drc_mode_map)/sizeof(drc_mode_map[0]); + +static XREF_T stereo_mode_map[] = +{ + {"off", MMAL_STEREOSCOPIC_MODE_NONE}, + {"sbs", MMAL_STEREOSCOPIC_MODE_SIDE_BY_SIDE}, + {"tb", MMAL_STEREOSCOPIC_MODE_TOP_BOTTOM}, +}; + +static const int stereo_mode_map_size = sizeof(stereo_mode_map)/sizeof(stereo_mode_map[0]); + + +#define CommandSharpness 0 +#define CommandContrast 1 +#define CommandBrightness 2 +#define CommandSaturation 3 +#define CommandISO 4 +#define CommandVideoStab 5 +#define CommandEVComp 6 +#define CommandExposure 7 +#define CommandAWB 8 +#define CommandImageFX 9 +#define CommandColourFX 10 +#define CommandMeterMode 11 +#define CommandRotation 12 +#define CommandHFlip 13 +#define CommandVFlip 14 +#define CommandROI 15 +#define CommandShutterSpeed 16 +#define CommandAwbGains 17 +#define CommandDRCLevel 18 +#define CommandStatsPass 19 +#define CommandAnnotate 20 +#define CommandStereoMode 21 +#define CommandStereoDecimate 22 +#define CommandStereoSwap 23 +#define CommandAnnotateExtras 24 + +static COMMAND_LIST cmdline_commands[] = +{ + {CommandSharpness, "-sharpness", "sh", "Set image sharpness (-100 to 100)", 1}, + {CommandContrast, "-contrast", "co", "Set image contrast (-100 to 100)", 1}, + {CommandBrightness, "-brightness","br", "Set image brightness (0 to 100)", 1}, + {CommandSaturation, "-saturation","sa", "Set image saturation (-100 to 100)", 1}, + {CommandISO, "-ISO", "ISO","Set capture ISO", 1}, + {CommandVideoStab, "-vstab", "vs", "Turn on video stabilisation", 0}, + {CommandEVComp, "-ev", "ev", "Set EV compensation - steps of 1/6 stop", 1}, + {CommandExposure, "-exposure", "ex", "Set exposure mode (see Notes)", 1}, + {CommandAWB, "-awb", "awb","Set AWB mode (see Notes)", 1}, + {CommandImageFX, "-imxfx", "ifx","Set image effect (see Notes)", 1}, + {CommandColourFX, "-colfx", "cfx","Set colour effect (U:V)", 1}, + {CommandMeterMode, "-metering", "mm", "Set metering mode (see Notes)", 1}, + {CommandRotation, "-rotation", "rot","Set image rotation (0-359)", 1}, + {CommandHFlip, "-hflip", "hf", "Set horizontal flip", 0}, + {CommandVFlip, "-vflip", "vf", "Set vertical flip", 0}, + {CommandROI, "-roi", "roi","Set region of interest (x,y,w,d as normalised coordinates [0.0-1.0])", 1}, + {CommandShutterSpeed,"-shutter", "ss", "Set shutter speed in microseconds", 1}, + {CommandAwbGains, "-awbgains", "awbg", "Set AWB gains - AWB mode must be off", 1}, + {CommandDRCLevel, "-drc", "drc", "Set DRC Level (see Notes)", 1}, + {CommandStatsPass, "-stats", "st", "Force recomputation of statistics on stills capture pass"}, + {CommandAnnotate, "-annotate", "a", "Enable/Set annotate flags or text", 1}, + {CommandStereoMode, "-stereo", "3d", "Select stereoscopic mode", 1}, + {CommandStereoDecimate,"-decimate","dec", "Half width/height of stereo image"}, + {CommandStereoSwap, "-3dswap", "3dswap", "Swap camera order for stereoscopic"}, + {CommandAnnotateExtras,"-annotateex","ae", "Set extra annotation parameters (text size, text colour(hex YUV), bg colour(hex YUV))", 2}, +}; + +static int cmdline_commands_size = sizeof(cmdline_commands) / sizeof(cmdline_commands[0]); + + +#define parameter_reset -99999 + +#define zoom_full_16P16 ((unsigned int)(65536 * 0.15)) +#define zoom_increment_16P16 (65536UL / 10) + +/** + * Update the passed in parameter according to the rest of the parameters + * passed in. + * + * + * @return 0 if reached end of cycle for this parameter, !0 otherwise + */ +static int update_cycle_parameter(int *option, int min, int max, int increment) +{ + vcos_assert(option); + if (!option) + return 0; + + if (*option == parameter_reset) + *option = min - increment; + + *option += increment; + + if (*option > max) + { + *option = parameter_reset; + return 0; + } + else + return 1; +} + + +/** + * Test/Demo code to cycle through a bunch of camera settings + * This code is pretty hacky so please don't complain!! + * It only does stuff that should have a visual impact (hence demo!) + * This will override any user supplied parameters + * + * Each call of this function will move on to the next setting + * + * @param camera Pointer to the camera to change settings on. + * @return 0 if reached end of complete sequence, !0 otherwise + */ + +int raspicamcontrol_cycle_test(MMAL_COMPONENT_T *camera) +{ + static int parameter = 0; + static int parameter_option = parameter_reset; // which value the parameter currently has + + vcos_assert(camera); + + // We are going to cycle through all the relevant entries in the parameter block + // and send options to the camera. + if (parameter == 0) + { + // sharpness + if (update_cycle_parameter(¶meter_option, -100, 100, 10)) + raspicamcontrol_set_sharpness(camera, parameter_option); + else + { + raspicamcontrol_set_sharpness(camera, 0); + parameter++; + } + } + else + if (parameter == 1) + { + // contrast + if (update_cycle_parameter(¶meter_option, -100, 100, 10)) + raspicamcontrol_set_contrast(camera, parameter_option); + else + { + raspicamcontrol_set_contrast(camera, 0); + parameter++; + } + } + else + if (parameter == 2) + { + // brightness + if (update_cycle_parameter(¶meter_option, 0, 100, 10)) + raspicamcontrol_set_brightness(camera, parameter_option); + else + { + raspicamcontrol_set_brightness(camera, 50); + parameter++; + } + } + else + if (parameter == 3) + { + // contrast + if (update_cycle_parameter(¶meter_option, -100, 100, 10)) + raspicamcontrol_set_saturation(camera, parameter_option); + else + { + parameter++; + raspicamcontrol_set_saturation(camera, 0); + } + } + else + if (parameter == 4) + { + // EV + if (update_cycle_parameter(¶meter_option, -10, 10, 4)) + raspicamcontrol_set_exposure_compensation(camera, parameter_option); + else + { + raspicamcontrol_set_exposure_compensation(camera, 0); + parameter++; + } + } + else + if (parameter == 5) + { + // MMAL_PARAM_EXPOSUREMODE_T + if (update_cycle_parameter(¶meter_option, 0, exposure_map_size, 1)) + raspicamcontrol_set_exposure_mode(camera, exposure_map[parameter_option].mmal_mode); + else + { + raspicamcontrol_set_exposure_mode(camera, MMAL_PARAM_EXPOSUREMODE_AUTO); + parameter++; + } + } + else + if (parameter == 6) + { + // MMAL_PARAM_AWB_T + if (update_cycle_parameter(¶meter_option, 0, awb_map_size, 1)) + raspicamcontrol_set_awb_mode(camera, awb_map[parameter_option].mmal_mode); + else + { + raspicamcontrol_set_awb_mode(camera, MMAL_PARAM_AWBMODE_AUTO); + parameter++; + } + } + if (parameter == 7) + { + // MMAL_PARAM_IMAGEFX_T + if (update_cycle_parameter(¶meter_option, 0, imagefx_map_size, 1)) + raspicamcontrol_set_imageFX(camera, imagefx_map[parameter_option].mmal_mode); + else + { + raspicamcontrol_set_imageFX(camera, MMAL_PARAM_IMAGEFX_NONE); + parameter++; + } + } + if (parameter == 8) + { + MMAL_PARAM_COLOURFX_T colfx = {0,0,0}; + switch (parameter_option) + { + case parameter_reset : + parameter_option = 1; + colfx.u = 128; + colfx.v = 128; + break; + case 1 : + parameter_option = 2; + colfx.u = 100; + colfx.v = 200; + break; + case 2 : + parameter_option = parameter_reset; + colfx.enable = 0; + parameter++; + break; + } + raspicamcontrol_set_colourFX(camera, &colfx); + } + + // Orientation + if (parameter == 9) + { + switch (parameter_option) + { + case parameter_reset: + raspicamcontrol_set_rotation(camera, 90); + parameter_option = 1; + break; + + case 1 : + raspicamcontrol_set_rotation(camera, 180); + parameter_option = 2; + break; + + case 2 : + raspicamcontrol_set_rotation(camera, 270); + parameter_option = 3; + break; + + case 3 : + { + raspicamcontrol_set_rotation(camera, 0); + raspicamcontrol_set_flips(camera, 1,0); + parameter_option = 4; + break; + } + case 4 : + { + raspicamcontrol_set_flips(camera, 0,1); + parameter_option = 5; + break; + } + case 5 : + { + raspicamcontrol_set_flips(camera, 1, 1); + parameter_option = 6; + break; + } + case 6 : + { + raspicamcontrol_set_flips(camera, 0, 0); + parameter_option = parameter_reset; + parameter++; + break; + } + } + } + + if (parameter == 10) + { + parameter = 1; + return 0; + } + + return 1; +} + + + +/** + * Convert string to the MMAL parameter for exposure mode + * @param str Incoming string to match + * @return MMAL parameter matching the string, or the AUTO option if no match found + */ +/*static*/ MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char *str) +{ + int i = raspicli_map_xref(str, exposure_map, exposure_map_size); + + if( i != -1) + return (MMAL_PARAM_EXPOSUREMODE_T)i; + + vcos_log_error("Unknown exposure mode: %s", str); + return MMAL_PARAM_EXPOSUREMODE_AUTO; +} + +/** + * Convert string to the MMAL parameter for AWB mode + * @param str Incoming string to match + * @return MMAL parameter matching the string, or the AUTO option if no match found + */ +/*static*/ MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char *str) +{ + int i = raspicli_map_xref(str, awb_map, awb_map_size); + + if( i != -1) + return (MMAL_PARAM_AWBMODE_T)i; + + vcos_log_error("Unknown awb mode: %s", str); + return MMAL_PARAM_AWBMODE_AUTO; +} + +/** + * Convert string to the MMAL parameter for image effects mode + * @param str Incoming string to match + * @return MMAL parameter matching the strong, or the AUTO option if no match found + */ +MMAL_PARAM_IMAGEFX_T imagefx_mode_from_string(const char *str) +{ + int i = raspicli_map_xref(str, imagefx_map, imagefx_map_size); + + if( i != -1) + return (MMAL_PARAM_IMAGEFX_T)i; + + vcos_log_error("Unknown image fx: %s", str); + return MMAL_PARAM_IMAGEFX_NONE; +} + +/** + * Convert string to the MMAL parameter for exposure metering mode + * @param str Incoming string to match + * @return MMAL parameter matching the string, or the AUTO option if no match found + */ +static MMAL_PARAM_EXPOSUREMETERINGMODE_T metering_mode_from_string(const char *str) +{ + int i = raspicli_map_xref(str, metering_mode_map, metering_mode_map_size); + + if( i != -1) + return (MMAL_PARAM_EXPOSUREMETERINGMODE_T)i; + + vcos_log_error("Unknown metering mode: %s", str); + return MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE; +} + +/** + * Convert string to the MMAL parameter for DRC level + * @param str Incoming string to match + * @return MMAL parameter matching the string, or the AUTO option if no match found + */ +static MMAL_PARAMETER_DRC_STRENGTH_T drc_mode_from_string(const char *str) +{ + int i = raspicli_map_xref(str, drc_mode_map, drc_mode_map_size); + + if( i != -1) + return (MMAL_PARAMETER_DRC_STRENGTH_T)i; + + vcos_log_error("Unknown DRC level: %s", str); + return MMAL_PARAMETER_DRC_STRENGTH_OFF; +} + +/** + * Convert string to the MMAL parameter for exposure metering mode + * @param str Incoming string to match + * @return MMAL parameter matching the string, or the AUTO option if no match found + */ +MMAL_STEREOSCOPIC_MODE_T stereo_mode_from_string(const char *str) +{ + int i = raspicli_map_xref(str, stereo_mode_map, stereo_mode_map_size); + + if( i != -1) + return (MMAL_STEREOSCOPIC_MODE_T)i; + + vcos_log_error("Unknown metering mode: %s", str); + return MMAL_STEREOSCOPIC_MODE_NONE; +} + +/** + * Parse a possible command pair - command and parameter + * @param arg1 Command + * @param arg2 Parameter (could be NULL) + * @return How many parameters were used, 0,1,2 + */ +int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2) +{ + int command_id, used = 0, num_parameters; + + if (!arg1) + return 0; + + command_id = raspicli_get_command_id(cmdline_commands, cmdline_commands_size, arg1, &num_parameters); + + // If invalid command, or we are missing a parameter, drop out + if (command_id==-1 || (command_id != -1 && num_parameters > 0 && arg2 == NULL)) + return 0; + + switch (command_id) + { + case CommandSharpness : // sharpness - needs single number parameter + sscanf(arg2, "%d", ¶ms->sharpness); + used = 2; + break; + + case CommandContrast : // contrast - needs single number parameter + sscanf(arg2, "%d", ¶ms->contrast); + used = 2; + break; + + case CommandBrightness : // brightness - needs single number parameter + sscanf(arg2, "%d", ¶ms->brightness); + used = 2; + break; + + case CommandSaturation : // saturation - needs single number parameter + sscanf(arg2, "%d", ¶ms->saturation); + used = 2; + break; + + case CommandISO : // ISO - needs single number parameter + sscanf(arg2, "%d", ¶ms->ISO); + used = 2; + break; + + case CommandVideoStab : // video stabilisation - if here, its on + params->videoStabilisation = 1; + used = 1; + break; + + case CommandEVComp : // EV - needs single number parameter + sscanf(arg2, "%d", ¶ms->exposureCompensation); + used = 2; + break; + + case CommandExposure : // exposure mode - needs string + params->exposureMode = exposure_mode_from_string(arg2); + used = 2; + break; + + case CommandAWB : // AWB mode - needs single number parameter + params->awbMode = awb_mode_from_string(arg2); + used = 2; + break; + + case CommandImageFX : // Image FX - needs string + params->imageEffect = imagefx_mode_from_string(arg2); + used = 2; + break; + + case CommandColourFX : // Colour FX - needs string "u:v" + sscanf(arg2, "%d:%d", ¶ms->colourEffects.u, ¶ms->colourEffects.v); + params->colourEffects.enable = 1; + used = 2; + break; + + case CommandMeterMode: + params->exposureMeterMode = metering_mode_from_string(arg2); + used = 2; + break; + + case CommandRotation : // Rotation - degree + sscanf(arg2, "%d", ¶ms->rotation); + used = 2; + break; + + case CommandHFlip : + params->hflip = 1; + used = 1; + break; + + case CommandVFlip : + params->vflip = 1; + used = 1; + break; + + case CommandROI : + { + double x,y,w,h; + int args; + + args = sscanf(arg2, "%lf,%lf,%lf,%lf", &x,&y,&w,&h); + + if (args != 4 || x > 1.0 || y > 1.0 || w > 1.0 || h > 1.0) + { + return 0; + } + + // Make sure we stay within bounds + if (x + w > 1.0) + w = 1 - x; + + if (y + h > 1.0) + h = 1 - y; + + params->roi.x = x; + params->roi.y = y; + params->roi.w = w; + params->roi.h = h; + + used = 2; + break; + } + + case CommandShutterSpeed : // Shutter speed needs single number parameter + { + sscanf(arg2, "%d", ¶ms->shutter_speed); + used = 2; + break; + } + + case CommandAwbGains : + { + double r,b; + int args; + + args = sscanf(arg2, "%lf,%lf", &r,&b); + + if (args != 2 || r > 8.0 || b > 8.0) + { + return 0; + } + + params->awb_gains_r = r; + params->awb_gains_b = b; + + used = 2; + break; + } + + case CommandDRCLevel: + { + params->drc_level = drc_mode_from_string(arg2); + used = 2; + break; + } + + case CommandStatsPass: + { + params->stats_pass = MMAL_TRUE; + used = 1; + break; + } + + case CommandAnnotate: + { + char dummy; + unsigned int bitmask; + // If parameter is a number, assume its a bitmask, otherwise a string + if (sscanf(arg2, "%u%c", &bitmask, &dummy) == 1) + { + params->enable_annotate |= bitmask; + } + else + { + params->enable_annotate |= ANNOTATE_USER_TEXT; + //copy string char by char and replace "\n" with newline character + unsigned char c; + char const *s = arg2; + char *t = ¶ms->annotate_string[0]; + int n=0; + while ((c = *s++) && n < MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3-1) + { + if (c == '\\' && *s) + { + switch (c = *s++) + { + case 'n': + c = '\n'; + break; + + default: + c = '\\'; + s--; + break; + } + } + *(t++) = c; + n++; + } + *t='\0'; + + //params->annotate_string[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3-1] = '\0'; + } + used=2; + break; + } + + case CommandAnnotateExtras: + { + // 3 parameters - text size (6-80), text colour (Hex VVUUYY) and background colour (Hex VVUUYY) + sscanf(arg2, "%u,%X,%X", ¶ms->annotate_text_size, + ¶ms->annotate_text_colour, + ¶ms->annotate_bg_colour); + used=2; + break; + } + + case CommandStereoMode: + { + params->stereo_mode.mode = stereo_mode_from_string(arg2); + used = 2; + break; + } + + case CommandStereoDecimate: + { + params->stereo_mode.decimate = MMAL_TRUE; + used = 1; + break; + } + + case CommandStereoSwap: + { + params->stereo_mode.swap_eyes = MMAL_TRUE; + used = 1; + break; + } + + } + + return used; +} + +/** + * Display help for command line options + */ +void raspicamcontrol_display_help() +{ + int i; + + fprintf(stdout, "\nImage parameter commands\n\n"); + + raspicli_display_help(cmdline_commands, cmdline_commands_size); + + fprintf(stdout, "\n\nNotes\n\nExposure mode options :\n%s", exposure_map[0].mode ); + + for (i=1;iexposureMode, exposure_map, exposure_map_size); + const char *awb_mode = raspicli_unmap_xref(params->awbMode, awb_map, awb_map_size); + const char *image_effect = raspicli_unmap_xref(params->imageEffect, imagefx_map, imagefx_map_size); + const char *metering_mode = raspicli_unmap_xref(params->exposureMeterMode, metering_mode_map, metering_mode_map_size); + + fprintf(stderr, "Sharpness %d, Contrast %d, Brightness %d\n", params->sharpness, params->contrast, params->brightness); + fprintf(stderr, "Saturation %d, ISO %d, Video Stabilisation %s, Exposure compensation %d\n", params->saturation, params->ISO, params->videoStabilisation ? "Yes": "No", params->exposureCompensation); + fprintf(stderr, "Exposure Mode '%s', AWB Mode '%s', Image Effect '%s'\n", exp_mode, awb_mode, image_effect); + fprintf(stderr, "Metering Mode '%s', Colour Effect Enabled %s with U = %d, V = %d\n", metering_mode, params->colourEffects.enable ? "Yes":"No", params->colourEffects.u, params->colourEffects.v); + fprintf(stderr, "Rotation %d, hflip %s, vflip %s\n", params->rotation, params->hflip ? "Yes":"No",params->vflip ? "Yes":"No"); + fprintf(stderr, "ROI x %lf, y %f, w %f h %f\n", params->roi.x, params->roi.y, params->roi.w, params->roi.h); +} + +/** + * Convert a MMAL status return value to a simple boolean of success + * ALso displays a fault if code is not success + * + * @param status The error code to convert + * @return 0 if status is success, 1 otherwise + */ +int mmal_status_to_int(MMAL_STATUS_T status) +{ + if (status == MMAL_SUCCESS) + return 0; + else + { + switch (status) + { + case MMAL_ENOMEM : vcos_log_error("Out of memory"); break; + case MMAL_ENOSPC : vcos_log_error("Out of resources (other than memory)"); break; + case MMAL_EINVAL: vcos_log_error("Argument is invalid"); break; + case MMAL_ENOSYS : vcos_log_error("Function not implemented"); break; + case MMAL_ENOENT : vcos_log_error("No such file or directory"); break; + case MMAL_ENXIO : vcos_log_error("No such device or address"); break; + case MMAL_EIO : vcos_log_error("I/O error"); break; + case MMAL_ESPIPE : vcos_log_error("Illegal seek"); break; + case MMAL_ECORRUPT : vcos_log_error("Data is corrupt \attention FIXME: not POSIX"); break; + case MMAL_ENOTREADY :vcos_log_error("Component is not ready \attention FIXME: not POSIX"); break; + case MMAL_ECONFIG : vcos_log_error("Component is not configured \attention FIXME: not POSIX"); break; + case MMAL_EISCONN : vcos_log_error("Port is already connected "); break; + case MMAL_ENOTCONN : vcos_log_error("Port is disconnected"); break; + case MMAL_EAGAIN : vcos_log_error("Resource temporarily unavailable. Try again later"); break; + case MMAL_EFAULT : vcos_log_error("Bad address"); break; + default : vcos_log_error("Unknown status error"); break; + } + + return 1; + } +} + +/** + * Give the supplied parameter block a set of default values + * @params Pointer to parameter block + */ +void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS *params) +{ + vcos_assert(params); + + params->sharpness = 0; + params->contrast = 0; + params->brightness = 50; + params->saturation = 0; + params->ISO = 0; // 0 = auto + params->videoStabilisation = 0; + params->exposureCompensation = 0; + params->exposureMode = MMAL_PARAM_EXPOSUREMODE_AUTO; + params->exposureMeterMode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE; + params->awbMode = MMAL_PARAM_AWBMODE_AUTO; + params->imageEffect = MMAL_PARAM_IMAGEFX_NONE; + params->colourEffects.enable = 0; + params->colourEffects.u = 128; + params->colourEffects.v = 128; + params->rotation = 0; + params->hflip = params->vflip = 0; + params->roi.x = params->roi.y = 0.0; + params->roi.w = params->roi.h = 1.0; + params->shutter_speed = 0; // 0 = auto + params->awb_gains_r = 0; // Only have any function if AWB OFF is used. + params->awb_gains_b = 0; + params->drc_level = MMAL_PARAMETER_DRC_STRENGTH_OFF; + params->stats_pass = MMAL_FALSE; + params->enable_annotate = 0; + params->annotate_string[0] = '\0'; + params->annotate_text_size = 0; //Use firmware default + params->annotate_text_colour = -1; //Use firmware default + params->annotate_bg_colour = -1; //Use firmware default + params->stereo_mode.mode = MMAL_STEREOSCOPIC_MODE_NONE; + params->stereo_mode.decimate = MMAL_FALSE; + params->stereo_mode.swap_eyes = MMAL_FALSE; +} + +/** + * Get all the current camera parameters from specified camera component + * @param camera Pointer to camera component + * @param params Pointer to parameter block to accept settings + * @return 0 if successful, non-zero if unsuccessful + */ +int raspicamcontrol_get_all_parameters(MMAL_COMPONENT_T *camera, RASPICAM_CAMERA_PARAMETERS *params) +{ + vcos_assert(camera); + vcos_assert(params); + + if (!camera || !params) + return 1; + +/* TODO : Write these get functions + params->sharpness = raspicamcontrol_get_sharpness(camera); + params->contrast = raspicamcontrol_get_contrast(camera); + params->brightness = raspicamcontrol_get_brightness(camera); + params->saturation = raspicamcontrol_get_saturation(camera); + params->ISO = raspicamcontrol_get_ISO(camera); + params->videoStabilisation = raspicamcontrol_get_video_stabilisation(camera); + params->exposureCompensation = raspicamcontrol_get_exposure_compensation(camera); + params->exposureMode = raspicamcontrol_get_exposure_mode(camera); + params->awbMode = raspicamcontrol_get_awb_mode(camera); + params->imageEffect = raspicamcontrol_get_image_effect(camera); + params->colourEffects = raspicamcontrol_get_colour_effect(camera); + params->thumbnailConfig = raspicamcontrol_get_thumbnail_config(camera); +*/ + return 0; +} + +/** + * Set the specified camera to all the specified settings + * @param camera Pointer to camera component + * @param params Pointer to parameter block containing parameters + * @return 0 if successful, none-zero if unsuccessful. + */ +int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T *camera, const RASPICAM_CAMERA_PARAMETERS *params) +{ + int result; + + result = raspicamcontrol_set_saturation(camera, params->saturation); + result += raspicamcontrol_set_sharpness(camera, params->sharpness); + result += raspicamcontrol_set_contrast(camera, params->contrast); + result += raspicamcontrol_set_brightness(camera, params->brightness); + result += raspicamcontrol_set_ISO(camera, params->ISO); + result += raspicamcontrol_set_video_stabilisation(camera, params->videoStabilisation); + result += raspicamcontrol_set_exposure_compensation(camera, params->exposureCompensation); + result += raspicamcontrol_set_exposure_mode(camera, params->exposureMode); + result += raspicamcontrol_set_metering_mode(camera, params->exposureMeterMode); + result += raspicamcontrol_set_awb_mode(camera, params->awbMode); + result += raspicamcontrol_set_awb_gains(camera, params->awb_gains_r, params->awb_gains_b); + result += raspicamcontrol_set_imageFX(camera, params->imageEffect); + result += raspicamcontrol_set_colourFX(camera, ¶ms->colourEffects); + //result += raspicamcontrol_set_thumbnail_parameters(camera, ¶ms->thumbnailConfig); TODO Not working for some reason + result += raspicamcontrol_set_rotation(camera, params->rotation); + result += raspicamcontrol_set_flips(camera, params->hflip, params->vflip); + result += raspicamcontrol_set_ROI(camera, params->roi); + result += raspicamcontrol_set_shutter_speed(camera, params->shutter_speed); + result += raspicamcontrol_set_DRC(camera, params->drc_level); + result += raspicamcontrol_set_stats_pass(camera, params->stats_pass); + result += raspicamcontrol_set_annotate(camera, params->enable_annotate, params->annotate_string, + params->annotate_text_size, + params->annotate_text_colour, + params->annotate_bg_colour); + + return result; +} + +/** + * Adjust the saturation level for images + * @param camera Pointer to camera component + * @param saturation Value to adjust, -100 to 100 + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_saturation(MMAL_COMPONENT_T *camera, int saturation) +{ + int ret = 0; + + if (!camera) + return 1; + + if (saturation >= -100 && saturation <= 100) + { + MMAL_RATIONAL_T value = {saturation, 100}; + ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_SATURATION, value)); + } + else + { + vcos_log_error("Invalid saturation value"); + ret = 1; + } + + return ret; +} + +/** + * Set the sharpness of the image + * @param camera Pointer to camera component + * @param sharpness Sharpness adjustment -100 to 100 + */ +int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T *camera, int sharpness) +{ + int ret = 0; + + if (!camera) + return 1; + + if (sharpness >= -100 && sharpness <= 100) + { + MMAL_RATIONAL_T value = {sharpness, 100}; + ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_SHARPNESS, value)); + } + else + { + vcos_log_error("Invalid sharpness value"); + ret = 1; + } + + return ret; +} + +/** + * Set the contrast adjustment for the image + * @param camera Pointer to camera component + * @param contrast Contrast adjustment -100 to 100 + * @return + */ +int raspicamcontrol_set_contrast(MMAL_COMPONENT_T *camera, int contrast) +{ + int ret = 0; + + if (!camera) + return 1; + + if (contrast >= -100 && contrast <= 100) + { + MMAL_RATIONAL_T value = {contrast, 100}; + ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_CONTRAST, value)); + } + else + { + vcos_log_error("Invalid contrast value"); + ret = 1; + } + + return ret; +} + +/** + * Adjust the brightness level for images + * @param camera Pointer to camera component + * @param brightness Value to adjust, 0 to 100 + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_brightness(MMAL_COMPONENT_T *camera, int brightness) +{ + int ret = 0; + + if (!camera) + return 1; + + if (brightness >= 0 && brightness <= 100) + { + MMAL_RATIONAL_T value = {brightness, 100}; + ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera->control, MMAL_PARAMETER_BRIGHTNESS, value)); + } + else + { + vcos_log_error("Invalid brightness value"); + ret = 1; + } + + return ret; +} + +/** + * Adjust the ISO used for images + * @param camera Pointer to camera component + * @param ISO Value to set TODO : + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_ISO(MMAL_COMPONENT_T *camera, int ISO) +{ + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_ISO, ISO)); +} + +/** + * Adjust the metering mode for images + * @param camera Pointer to camera component + * @param saturation Value from following + * - MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE, + * - MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT, + * - MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT, + * - MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T m_mode ) +{ + MMAL_PARAMETER_EXPOSUREMETERINGMODE_T meter_mode = {{MMAL_PARAMETER_EXP_METERING_MODE,sizeof(meter_mode)}, + m_mode}; + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, &meter_mode.hdr)); +} + + +/** + * Set the video stabilisation flag. Only used in video mode + * @param camera Pointer to camera component + * @param saturation Flag 0 off 1 on + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T *camera, int vstabilisation) +{ + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set_boolean(camera->control, MMAL_PARAMETER_VIDEO_STABILISATION, vstabilisation)); +} + +/** + * Adjust the exposure compensation for images (EV) + * @param camera Pointer to camera component + * @param exp_comp Value to adjust, -10 to +10 + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T *camera, int exp_comp) +{ + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set_int32(camera->control, MMAL_PARAMETER_EXPOSURE_COMP , exp_comp)); +} + + +/** + * Set exposure mode for images + * @param camera Pointer to camera component + * @param mode Exposure mode to set from + * - MMAL_PARAM_EXPOSUREMODE_OFF, + * - MMAL_PARAM_EXPOSUREMODE_AUTO, + * - MMAL_PARAM_EXPOSUREMODE_NIGHT, + * - MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, + * - MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, + * - MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, + * - MMAL_PARAM_EXPOSUREMODE_SPORTS, + * - MMAL_PARAM_EXPOSUREMODE_SNOW, + * - MMAL_PARAM_EXPOSUREMODE_BEACH, + * - MMAL_PARAM_EXPOSUREMODE_VERYLONG, + * - MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, + * - MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, + * - MMAL_PARAM_EXPOSUREMODE_FIREWORKS, + * + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_EXPOSUREMODE_T mode) +{ + MMAL_PARAMETER_EXPOSUREMODE_T exp_mode = {{MMAL_PARAMETER_EXPOSURE_MODE,sizeof(exp_mode)}, mode}; + + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, &exp_mode.hdr)); +} + + +/** + * Set the aWB (auto white balance) mode for images + * @param camera Pointer to camera component + * @param awb_mode Value to set from + * - MMAL_PARAM_AWBMODE_OFF, + * - MMAL_PARAM_AWBMODE_AUTO, + * - MMAL_PARAM_AWBMODE_SUNLIGHT, + * - MMAL_PARAM_AWBMODE_CLOUDY, + * - MMAL_PARAM_AWBMODE_SHADE, + * - MMAL_PARAM_AWBMODE_TUNGSTEN, + * - MMAL_PARAM_AWBMODE_FLUORESCENT, + * - MMAL_PARAM_AWBMODE_INCANDESCENT, + * - MMAL_PARAM_AWBMODE_FLASH, + * - MMAL_PARAM_AWBMODE_HORIZON, + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T *camera, MMAL_PARAM_AWBMODE_T awb_mode) +{ + MMAL_PARAMETER_AWBMODE_T param = {{MMAL_PARAMETER_AWB_MODE,sizeof(param)}, awb_mode}; + + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, ¶m.hdr)); +} + +int raspicamcontrol_set_awb_gains(MMAL_COMPONENT_T *camera, float r_gain, float b_gain) +{ + MMAL_PARAMETER_AWB_GAINS_T param = {{MMAL_PARAMETER_CUSTOM_AWB_GAINS,sizeof(param)}, {0,0}, {0,0}}; + + if (!camera) + return 1; + + if (!r_gain || !b_gain) + return 0; + + param.r_gain.num = (unsigned int)(r_gain * 65536); + param.b_gain.num = (unsigned int)(b_gain * 65536); + param.r_gain.den = param.b_gain.den = 65536; + return mmal_status_to_int(mmal_port_parameter_set(camera->control, ¶m.hdr)); +} + +/** + * Set the image effect for the images + * @param camera Pointer to camera component + * @param imageFX Value from + * - MMAL_PARAM_IMAGEFX_NONE, + * - MMAL_PARAM_IMAGEFX_NEGATIVE, + * - MMAL_PARAM_IMAGEFX_SOLARIZE, + * - MMAL_PARAM_IMAGEFX_POSTERIZE, + * - MMAL_PARAM_IMAGEFX_WHITEBOARD, + * - MMAL_PARAM_IMAGEFX_BLACKBOARD, + * - MMAL_PARAM_IMAGEFX_SKETCH, + * - MMAL_PARAM_IMAGEFX_DENOISE, + * - MMAL_PARAM_IMAGEFX_EMBOSS, + * - MMAL_PARAM_IMAGEFX_OILPAINT, + * - MMAL_PARAM_IMAGEFX_HATCH, + * - MMAL_PARAM_IMAGEFX_GPEN, + * - MMAL_PARAM_IMAGEFX_PASTEL, + * - MMAL_PARAM_IMAGEFX_WATERCOLOUR, + * - MMAL_PARAM_IMAGEFX_FILM, + * - MMAL_PARAM_IMAGEFX_BLUR, + * - MMAL_PARAM_IMAGEFX_SATURATION, + * - MMAL_PARAM_IMAGEFX_COLOURSWAP, + * - MMAL_PARAM_IMAGEFX_WASHEDOUT, + * - MMAL_PARAM_IMAGEFX_POSTERISE, + * - MMAL_PARAM_IMAGEFX_COLOURPOINT, + * - MMAL_PARAM_IMAGEFX_COLOURBALANCE, + * - MMAL_PARAM_IMAGEFX_CARTOON, + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T *camera, MMAL_PARAM_IMAGEFX_T imageFX) +{ + MMAL_PARAMETER_IMAGEFX_T imgFX = {{MMAL_PARAMETER_IMAGE_EFFECT,sizeof(imgFX)}, imageFX}; + + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, &imgFX.hdr)); +} + +/* TODO :what to do with the image effects parameters? + MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imfx_param = {{MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,sizeof(imfx_param)}, + imageFX, 0, {0}}; +mmal_port_parameter_set(camera->control, &imfx_param.hdr); + */ + +/** + * Set the colour effect for images (Set UV component) + * @param camera Pointer to camera component + * @param colourFX Contains enable state and U and V numbers to set (e.g. 128,128 = Black and white) + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T *camera, const MMAL_PARAM_COLOURFX_T *colourFX) +{ + MMAL_PARAMETER_COLOURFX_T colfx = {{MMAL_PARAMETER_COLOUR_EFFECT,sizeof(colfx)}, 0, 0, 0}; + + if (!camera) + return 1; + + colfx.enable = colourFX->enable; + colfx.u = colourFX->u; + colfx.v = colourFX->v; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, &colfx.hdr)); + +} + + +/** + * Set the rotation of the image + * @param camera Pointer to camera component + * @param rotation Degree of rotation (any number, but will be converted to 0,90,180 or 270 only) + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_rotation(MMAL_COMPONENT_T *camera, int rotation) +{ + int ret; + int my_rotation = ((rotation % 360 ) / 90) * 90; + + ret = mmal_port_parameter_set_int32(camera->output[0], MMAL_PARAMETER_ROTATION, my_rotation); + mmal_port_parameter_set_int32(camera->output[1], MMAL_PARAMETER_ROTATION, my_rotation); + mmal_port_parameter_set_int32(camera->output[2], MMAL_PARAMETER_ROTATION, my_rotation); + + return ret; +} + +/** + * Set the flips state of the image + * @param camera Pointer to camera component + * @param hflip If true, horizontally flip the image + * @param vflip If true, vertically flip the image + * + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_flips(MMAL_COMPONENT_T *camera, int hflip, int vflip) +{ + MMAL_PARAMETER_MIRROR_T mirror = {{MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T)}, MMAL_PARAM_MIRROR_NONE}; + + if (hflip && vflip) + mirror.value = MMAL_PARAM_MIRROR_BOTH; + else + if (hflip) + mirror.value = MMAL_PARAM_MIRROR_HORIZONTAL; + else + if (vflip) + mirror.value = MMAL_PARAM_MIRROR_VERTICAL; + + mmal_port_parameter_set(camera->output[0], &mirror.hdr); + mmal_port_parameter_set(camera->output[1], &mirror.hdr); + return mmal_port_parameter_set(camera->output[2], &mirror.hdr); +} + +/** + * Set the ROI of the sensor to use for captures/preview + * @param camera Pointer to camera component + * @param rect Normalised coordinates of ROI rectangle + * + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_ROI(MMAL_COMPONENT_T *camera, PARAM_FLOAT_RECT_T rect) +{ + MMAL_PARAMETER_INPUT_CROP_T crop = {{MMAL_PARAMETER_INPUT_CROP, sizeof(MMAL_PARAMETER_INPUT_CROP_T)}}; + + crop.rect.x = (65536 * rect.x); + crop.rect.y = (65536 * rect.y); + crop.rect.width = (65536 * rect.w); + crop.rect.height = (65536 * rect.h); + + return mmal_port_parameter_set(camera->control, &crop.hdr); +} + +/** + * Zoom in and Zoom out by changing ROI + * @param camera Pointer to camera component + * @param zoom_command zoom command enum + * @return 0 if successful, non-zero otherwise + */ +int raspicamcontrol_zoom_in_zoom_out(MMAL_COMPONENT_T *camera, ZOOM_COMMAND_T zoom_command, PARAM_FLOAT_RECT_T *roi) { + MMAL_PARAMETER_INPUT_CROP_T crop; + crop.hdr.id = MMAL_PARAMETER_INPUT_CROP; + crop.hdr.size = sizeof(crop); + + if (mmal_port_parameter_get(camera->control, &crop.hdr) != MMAL_SUCCESS) + { + vcos_log_error("mmal_port_parameter_get(camera->control, &crop.hdr) failed, skip it"); + return 0; + } + + if (zoom_command == ZOOM_IN) + { + if (crop.rect.width <= (zoom_full_16P16 + zoom_increment_16P16)) + { + crop.rect.width = zoom_full_16P16; + crop.rect.height = zoom_full_16P16; + } + else + { + crop.rect.width -= zoom_increment_16P16; + crop.rect.height -= zoom_increment_16P16; + } + } + else if (zoom_command == ZOOM_OUT) + { + unsigned int increased_size = crop.rect.width + zoom_increment_16P16; + if (increased_size < crop.rect.width) //overflow + { + crop.rect.width = 65536; + crop.rect.height = 65536; + } + else + { + crop.rect.width = increased_size; + crop.rect.height = increased_size; + } + } + + if (zoom_command == ZOOM_RESET) + { + crop.rect.x = 0; + crop.rect.y = 0; + crop.rect.width = 65536; + crop.rect.height = 65536; + } + else + { + unsigned int centered_top_coordinate = (65536 - crop.rect.width) / 2; + crop.rect.x = centered_top_coordinate; + crop.rect.y = centered_top_coordinate; + } + + int ret = mmal_status_to_int(mmal_port_parameter_set(camera->control, &crop.hdr)); + + if (ret == 0) { + roi->x = roi->y = (double)crop.rect.x/65536; + roi->w = roi->h = (double)crop.rect.width/65536; + } + else + { + vcos_log_error("Failed to set crop values, x/y: %u, w/h: %u", crop.rect.x, crop.rect.width); + ret = 1; + } + + return ret; +} + +/** + * Adjust the exposure time used for images + * @param camera Pointer to camera component + * @param shutter speed in microseconds + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T *camera, int speed) +{ + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set_uint32(camera->control, MMAL_PARAMETER_SHUTTER_SPEED, speed)); +} + +/** + * Adjust the Dynamic range compression level + * @param camera Pointer to camera component + * @param strength Strength of DRC to apply + * MMAL_PARAMETER_DRC_STRENGTH_OFF + * MMAL_PARAMETER_DRC_STRENGTH_LOW + * MMAL_PARAMETER_DRC_STRENGTH_MEDIUM + * MMAL_PARAMETER_DRC_STRENGTH_HIGH + * + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_DRC(MMAL_COMPONENT_T *camera, MMAL_PARAMETER_DRC_STRENGTH_T strength) +{ + MMAL_PARAMETER_DRC_T drc = {{MMAL_PARAMETER_DYNAMIC_RANGE_COMPRESSION, sizeof(MMAL_PARAMETER_DRC_T)}, strength}; + + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, &drc.hdr)); +} + +int raspicamcontrol_set_stats_pass(MMAL_COMPONENT_T *camera, int stats_pass) +{ + if (!camera) + return 1; + + return mmal_status_to_int(mmal_port_parameter_set_boolean(camera->control, MMAL_PARAMETER_CAPTURE_STATS_PASS, stats_pass)); +} + + +/** + * Set the annotate data + * @param camera Pointer to camera component + * @param Bitmask of required annotation data. 0 for off. + * @param If set, a pointer to text string to use instead of bitmask, max length 32 characters + * + * @return 0 if successful, non-zero if any parameters out of range + */ +int raspicamcontrol_set_annotate(MMAL_COMPONENT_T *camera, const int settings, const char *string, + const int text_size, const int text_colour, const int bg_colour) +{ + MMAL_PARAMETER_CAMERA_ANNOTATE_V3_T annotate = + {{MMAL_PARAMETER_ANNOTATE, sizeof(MMAL_PARAMETER_CAMERA_ANNOTATE_V3_T)}}; + + if (settings) + { + time_t t = time(NULL); + struct tm tm = *localtime(&t); + char tmp[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3]; + int process_datetime = 1; + + annotate.enable = 1; + + if (settings & (ANNOTATE_APP_TEXT | ANNOTATE_USER_TEXT)) + { + if ((settings & (ANNOTATE_TIME_TEXT | ANNOTATE_DATE_TEXT)) && strchr(string,'%') != NULL) + { //string contains strftime parameter? + strftime(annotate.text, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3, string, &tm ); + process_datetime = 0; + }else{ + strncpy(annotate.text, string, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3); + } + annotate.text[MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3-1] = '\0'; + } + + if (process_datetime && (settings & ANNOTATE_TIME_TEXT)) + { + if(strlen(annotate.text)){ + strftime(tmp, 32, " %X", &tm ); + }else{ + strftime(tmp, 32, "%X", &tm ); + } + strncat(annotate.text, tmp, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3 - strlen(annotate.text) - 1); + } + + if (process_datetime && (settings & ANNOTATE_DATE_TEXT)) + { + if(strlen(annotate.text)){ + strftime(tmp, 32, " %x", &tm ); + }else{ + strftime(tmp, 32, "%x", &tm ); + } + strncat(annotate.text, tmp, MMAL_CAMERA_ANNOTATE_MAX_TEXT_LEN_V3 - strlen(annotate.text) - 1); + } + + if (settings & ANNOTATE_SHUTTER_SETTINGS) + annotate.show_shutter = MMAL_TRUE; + + if (settings & ANNOTATE_GAIN_SETTINGS) + annotate.show_analog_gain = MMAL_TRUE; + + if (settings & ANNOTATE_LENS_SETTINGS) + annotate.show_lens = MMAL_TRUE; + + if (settings & ANNOTATE_CAF_SETTINGS) + annotate.show_caf = MMAL_TRUE; + + if (settings & ANNOTATE_MOTION_SETTINGS) + annotate.show_motion = MMAL_TRUE; + + if (settings & ANNOTATE_FRAME_NUMBER) + annotate.show_frame_num = MMAL_TRUE; + + if (settings & ANNOTATE_BLACK_BACKGROUND) + annotate.enable_text_background = MMAL_TRUE; + + annotate.text_size = text_size; + + if (text_colour != -1) + { + annotate.custom_text_colour = MMAL_TRUE; + annotate.custom_text_Y = text_colour&0xff; + annotate.custom_text_U = (text_colour>>8)&0xff; + annotate.custom_text_V = (text_colour>>16)&0xff; + } + else + annotate.custom_text_colour = MMAL_FALSE; + + if (bg_colour != -1) + { + annotate.custom_background_colour = MMAL_TRUE; + annotate.custom_background_Y = bg_colour&0xff; + annotate.custom_background_U = (bg_colour>>8)&0xff; + annotate.custom_background_V = (bg_colour>>16)&0xff; + } + else + annotate.custom_background_colour = MMAL_FALSE; + } + else + annotate.enable = 0; + + return mmal_status_to_int(mmal_port_parameter_set(camera->control, &annotate.hdr)); +} + +int raspicamcontrol_set_stereo_mode(MMAL_PORT_T *port, MMAL_PARAMETER_STEREOSCOPIC_MODE_T *stereo_mode) +{ + MMAL_PARAMETER_STEREOSCOPIC_MODE_T stereo = { {MMAL_PARAMETER_STEREOSCOPIC_MODE, sizeof(stereo)}, + MMAL_STEREOSCOPIC_MODE_NONE, MMAL_FALSE, MMAL_FALSE }; + if (stereo_mode->mode != MMAL_STEREOSCOPIC_MODE_NONE) + { + stereo.mode = stereo_mode->mode; + stereo.decimate = stereo_mode->decimate; + stereo.swap_eyes = stereo_mode->swap_eyes; + } + return mmal_status_to_int(mmal_port_parameter_set(port, &stereo.hdr)); +} + +/** + * Asked GPU how much memory it has allocated + * + * @return amount of memory in MB + */ +static int raspicamcontrol_get_mem_gpu(void) +{ + char response[80] = ""; + int gpu_mem = 0; + if (vc_gencmd(response, sizeof response, "get_mem gpu") == 0) + vc_gencmd_number_property(response, "gpu", &gpu_mem); + return gpu_mem; +} + +/** + * Ask GPU about its camera abilities + * @param supported None-zero if software supports the camera + * @param detected None-zero if a camera has been detected + */ +static void raspicamcontrol_get_camera(int *supported, int *detected) +{ + char response[80] = ""; + if (vc_gencmd(response, sizeof response, "get_camera") == 0) + { + if (supported) + vc_gencmd_number_property(response, "supported", supported); + if (detected) + vc_gencmd_number_property(response, "detected", detected); + } +} + +/** + * Check to see if camera is supported, and we have allocated enough meooryAsk GPU about its camera abilities + * @param supported None-zero if software supports the camera + * @param detected None-zero if a camera has been detected + */ +void raspicamcontrol_check_configuration(int min_gpu_mem) +{ + int gpu_mem = raspicamcontrol_get_mem_gpu(); + int supported = 0, detected = 0; + raspicamcontrol_get_camera(&supported, &detected); + if (!supported) + vcos_log_error("Camera is not enabled in this build. Try running \"sudo raspi-config\" and ensure that \"camera\" has been enabled\n"); + else if (gpu_mem < min_gpu_mem) + vcos_log_error("Only %dM of gpu_mem is configured. Try running \"sudo raspi-config\" and ensure that \"memory_split\" has a value of %d or greater\n", gpu_mem, min_gpu_mem); + else if (!detected) + vcos_log_error("Camera is not detected. Please check carefully the camera module is installed correctly\n"); + else + vcos_log_error("Failed to run camera app. Please check for firmware updates\n"); +} + From fa2adca74cbf868c17cc5b62cba95152df778918 Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:43:54 +0400 Subject: [PATCH 04/18] some changes according to updated api --- src/raspicam_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/raspicam_node.cpp b/src/raspicam_node.cpp index 85d9de7..200802c 100644 --- a/src/raspicam_node.cpp +++ b/src/raspicam_node.cpp @@ -374,7 +374,7 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu PORT_USERDATA* pData = port->userdata; if (pData && pData->pstate.isInit) { size_t bytes_written = buffer->length; - if (buffer->length) { + if (buffer->length > 0) { if (pData->id != INT_MAX) { if (pData->id + buffer->length > IMG_BUFFER_SIZE) { ROS_ERROR("pData->id (%d) + buffer->length (%d) > " From d3a96216141a3c7acedec82d7335fb6b2cba294b Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:44:06 +0400 Subject: [PATCH 05/18] some changes according to updated api --- src/raspicam_node.cpp | 58 +++++++++++++++++++++++++++++-------------- 1 file changed, 40 insertions(+), 18 deletions(-) diff --git a/src/raspicam_node.cpp b/src/raspicam_node.cpp index 200802c..0edcc01 100644 --- a/src/raspicam_node.cpp +++ b/src/raspicam_node.cpp @@ -71,9 +71,9 @@ int main(int argc, char** argv) { #include "raspicam_node/MotionVectors.h" #include #include - +extern "C" { #include "RaspiCamControl.h" - +} #include #include @@ -141,8 +141,8 @@ typedef struct MMAL_PORT_USERDATA_T { RASPIVID_STATE& pstate; // pointer to our state for use by callback bool abort; // Set to 1 in callback if an error occurs to attempt to abort // the capture - int frame; - int id; + size_t frame; + size_t id; int frames_skipped = 0; } PORT_USERDATA; @@ -194,7 +194,7 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { nh.param("camera_id", state.camera_id, 0); // Set up the camera_parameters to default - raspicamcontrol_set_defaults(state.camera_parameters); + raspicamcontrol_set_defaults(&state.camera_parameters); bool temp; nh.param("hFlip", temp, false); @@ -415,6 +415,7 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu image.msg.height = pData->pstate.height; image.msg.width = pData->pstate.width; image.msg.step = (pData->pstate.width * 3); + // ROS_INFO("pData->id = %lu, height = %d, width = %d, size = %lu, frame = %lu\n", pData->id - buffer->offset, pData->pstate.height, pData->pstate.width, pData->pstate.height*pData->pstate.width * 3, pData->frame & 1); auto start = pData->buffer[pData->frame & 1].get(); auto end = &(pData->buffer[pData->frame & 1].get()[pData->id]); image.msg.data.resize(pData->id); @@ -469,6 +470,27 @@ static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) { goto error; } + state.camera_parameters.stereo_mode.mode = stereo_mode_from_string("tb"); + status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[0], &state.camera_parameters.stereo_mode)); + if (status != MMAL_SUCCESS) + { + vcos_log_error("Could not set stereo mode : error %d", status); + goto error; + } + status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[1], &state.camera_parameters.stereo_mode)); + + if (status != MMAL_SUCCESS) + { + vcos_log_error("Could not set stereo mode : error %d", status); + goto error; + } + status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[2], &state.camera_parameters.stereo_mode)); + + if (status != MMAL_SUCCESS) + { + vcos_log_error("Could not set stereo mode : error %d", status); + goto error; + } if (!camera->output_num) { vcos_log_error("Camera doesn't have output ports"); ROS_ERROR("Camera doesn't have output ports"); @@ -606,7 +628,7 @@ static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) { goto error; } - raspicamcontrol_set_all_parameters(*camera, state.camera_parameters); + raspicamcontrol_set_all_parameters(camera, &state.camera_parameters); state.camera_component.reset(camera); @@ -1285,22 +1307,22 @@ void reconfigure_callback(raspicam_node::CameraConfig& config, uint32_t level, R PARAM_FLOAT_RECT_T roi; roi.x = roi.y = offset; roi.w = roi.h = size; - raspicamcontrol_set_ROI(*state.camera_component, roi); + raspicamcontrol_set_ROI(state.camera_component.get(), roi); } - raspicamcontrol_set_exposure_mode(*state.camera_component, exposure_mode_from_string(config.exposure_mode.c_str())); + raspicamcontrol_set_exposure_mode(state.camera_component.get(), exposure_mode_from_string(config.exposure_mode.c_str())); - raspicamcontrol_set_awb_mode(*state.camera_component, awb_mode_from_string(config.awb_mode.c_str())); + raspicamcontrol_set_awb_mode(state.camera_component.get(), awb_mode_from_string(config.awb_mode.c_str())); - raspicamcontrol_set_contrast(*state.camera_component, config.contrast); - raspicamcontrol_set_sharpness(*state.camera_component, config.sharpness); - raspicamcontrol_set_brightness(*state.camera_component, config.brightness); - raspicamcontrol_set_saturation(*state.camera_component, config.saturation); - raspicamcontrol_set_ISO(*state.camera_component, config.ISO); - raspicamcontrol_set_exposure_compensation(*state.camera_component, config.exposure_compensation); - raspicamcontrol_set_video_stabilisation(*state.camera_component, config.video_stabilisation); - raspicamcontrol_set_flips(*state.camera_component, config.hFlip, config.vFlip); - raspicamcontrol_set_shutter_speed(*state.camera_component, config.shutter_speed); + raspicamcontrol_set_contrast(state.camera_component.get(), config.contrast); + raspicamcontrol_set_sharpness(state.camera_component.get(), config.sharpness); + raspicamcontrol_set_brightness(state.camera_component.get(), config.brightness); + raspicamcontrol_set_saturation(state.camera_component.get(), config.saturation); + raspicamcontrol_set_ISO(state.camera_component.get(), config.ISO); + raspicamcontrol_set_exposure_compensation(state.camera_component.get(), config.exposure_compensation); + raspicamcontrol_set_video_stabilisation(state.camera_component.get(), config.video_stabilisation); + raspicamcontrol_set_flips(state.camera_component.get(), config.hFlip, config.vFlip); + raspicamcontrol_set_shutter_speed(state.camera_component.get(), config.shutter_speed); ROS_DEBUG("Reconfigure done"); } From 5e8d98c58aa8c63c77d0f8f817a5f606171647cc Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:45:04 +0400 Subject: [PATCH 06/18] new target stereo_node and some optimization and debugging tools --- CMakeLists.txt | 34 ++++++++++++++++++++++++++++++---- 1 file changed, 30 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9478576..5f38de7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,11 @@ project(raspicam_node) set(CMAKE_CXX_STANDARD 14) # use C++14 +# Enable compile optimizations +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") +set(CMAKE_BUILD_TYPE Release) + +# add_definitions(-D_DEBUG) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -43,7 +48,7 @@ if(${CMAKE_SYSTEM_PROCESSOR} MATCHES arm*) IF( (NOT vchostif_LIBS )) MESSAGE(FATAL_ERROR "Could not find vchostif library") ENDIF() - + set(RPI_LIBRARIES ${RPI_LIBRARIES} ${bcmhost_LIBS} ${vcos_LIBS} @@ -121,18 +126,39 @@ include_directories(include add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral) -## Declare a cpp executable -add_executable(raspicam_node src/raspicam_node.cpp src/RaspiCamControl.cpp) +## Declare a cpp library +add_library(raspicli STATIC + src/RaspiCLI.c +) + +add_library(raspicamcontrol STATIC + src/RaspiCamControl.c +) + +## Declare a cpp executables +add_executable(raspicam_node src/raspicam_node.cpp) +add_executable(stereo_node src/stereo_node.cpp) + ## Add cmake target dependencies of the executable/library add_dependencies(raspicam_node raspicam_node_generate_messages_cpp) add_dependencies(raspicam_node raspicam_node_gencfg) +add_dependencies(stereo_node raspicam_node_generate_messages_cpp) +add_dependencies(stereo_node raspicam_node_gencfg) ## Specify libraries to link a library or executable target against target_link_libraries(raspicam_node + raspicamcontrol + raspicli ${catkin_LIBRARIES} ${RPI_LIBRARIES} ) + target_link_libraries(stereo_node + raspicamcontrol + raspicli + ${catkin_LIBRARIES} + ${RPI_LIBRARIES} +) ############# ## Install ## @@ -149,7 +175,7 @@ target_link_libraries(raspicam_node # ) ## Mark executables and/or libraries for installation -install(TARGETS raspicam_node +install(TARGETS raspicam_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} From 10487717316e544253f3be7f3c08f8485bedd6b3 Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:45:55 +0400 Subject: [PATCH 07/18] ingor vscode --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 72ba949..5a7684c 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ *.o *.so *.sublime* +/.vscode/ From 37989ba8fc182870ee478c1967d3bf3eb077dd08 Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 01:46:06 +0400 Subject: [PATCH 08/18] old api --- src/RaspiCamControl.cpp | 922 ---------------------------------------- 1 file changed, 922 deletions(-) delete mode 100644 src/RaspiCamControl.cpp diff --git a/src/RaspiCamControl.cpp b/src/RaspiCamControl.cpp deleted file mode 100644 index 3743ba0..0000000 --- a/src/RaspiCamControl.cpp +++ /dev/null @@ -1,922 +0,0 @@ -/* -Copyright (c) 2013, Broadcom Europe Ltd -Copyright (c) 2013, James Hughes -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of the copyright holder nor the - names of its contributors may be used to endorse or promote products - derived from this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*/ - -#ifdef __arm__ - -#include -#include - -#include "interface/vcos/vcos.h" - -#include "RaspiCamControl.h" -#include "interface/mmal/mmal.h" -#include "interface/mmal/mmal_logging.h" -#include "interface/mmal/util/mmal_default_components.h" -#include "interface/mmal/util/mmal_util.h" -#include "interface/mmal/util/mmal_util_params.h" -#include "interface/vmcs_host/vc_vchi_gencmd.h" - -/// Cross reference structure, mode string against mode id -typedef struct xref_t { - char const* mode; - int mmal_mode; -} XREF_T; - -/// Structure to cross reference exposure strings against the MMAL parameter -/// equivalent -static XREF_T exposure_map[] = { { "off", MMAL_PARAM_EXPOSUREMODE_OFF }, - { "auto", MMAL_PARAM_EXPOSUREMODE_AUTO }, - { "night", MMAL_PARAM_EXPOSUREMODE_NIGHT }, - { "nightpreview", MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW }, - { "backlight", MMAL_PARAM_EXPOSUREMODE_BACKLIGHT }, - { "spotlight", MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT }, - { "sports", MMAL_PARAM_EXPOSUREMODE_SPORTS }, - { "snow", MMAL_PARAM_EXPOSUREMODE_SNOW }, - { "beach", MMAL_PARAM_EXPOSUREMODE_BEACH }, - { "verylong", MMAL_PARAM_EXPOSUREMODE_VERYLONG }, - { "fixedfps", MMAL_PARAM_EXPOSUREMODE_FIXEDFPS }, - { "antishake", MMAL_PARAM_EXPOSUREMODE_ANTISHAKE }, - { "fireworks", MMAL_PARAM_EXPOSUREMODE_FIREWORKS } }; - -static const int exposure_map_size = sizeof(exposure_map) / sizeof(exposure_map[0]); - -/// Structure to cross reference awb strings against the MMAL parameter -/// equivalent -static XREF_T awb_map[] = { { "off", MMAL_PARAM_AWBMODE_OFF }, - { "auto", MMAL_PARAM_AWBMODE_AUTO }, - { "sun", MMAL_PARAM_AWBMODE_SUNLIGHT }, - { "cloud", MMAL_PARAM_AWBMODE_CLOUDY }, - { "shade", MMAL_PARAM_AWBMODE_SHADE }, - { "tungsten", MMAL_PARAM_AWBMODE_TUNGSTEN }, - { "fluorescent", MMAL_PARAM_AWBMODE_FLUORESCENT }, - { "incandescent", MMAL_PARAM_AWBMODE_INCANDESCENT }, - { "flash", MMAL_PARAM_AWBMODE_FLASH }, - { "horizon", MMAL_PARAM_AWBMODE_HORIZON } }; - -static const int awb_map_size = sizeof(awb_map) / sizeof(awb_map[0]); - -/// Structure to cross reference image effect against the MMAL parameter -/// equivalent -static XREF_T imagefx_map[] = { { "none", MMAL_PARAM_IMAGEFX_NONE }, - { "negative", MMAL_PARAM_IMAGEFX_NEGATIVE }, - { "solarise", MMAL_PARAM_IMAGEFX_SOLARIZE }, - { "sketch", MMAL_PARAM_IMAGEFX_SKETCH }, - { "denoise", MMAL_PARAM_IMAGEFX_DENOISE }, - { "emboss", MMAL_PARAM_IMAGEFX_EMBOSS }, - { "oilpaint", MMAL_PARAM_IMAGEFX_OILPAINT }, - { "hatch", MMAL_PARAM_IMAGEFX_HATCH }, - { "gpen", MMAL_PARAM_IMAGEFX_GPEN }, - { "pastel", MMAL_PARAM_IMAGEFX_PASTEL }, - { "watercolour", MMAL_PARAM_IMAGEFX_WATERCOLOUR }, - { "film", MMAL_PARAM_IMAGEFX_FILM }, - { "blur", MMAL_PARAM_IMAGEFX_BLUR }, - { "saturation", MMAL_PARAM_IMAGEFX_SATURATION }, - { "colourswap", MMAL_PARAM_IMAGEFX_COLOURSWAP }, - { "washedout", MMAL_PARAM_IMAGEFX_WASHEDOUT }, - { "posterise", MMAL_PARAM_IMAGEFX_POSTERISE }, - { "colourpoint", MMAL_PARAM_IMAGEFX_COLOURPOINT }, - { "colourbalance", MMAL_PARAM_IMAGEFX_COLOURBALANCE }, - { "cartoon", MMAL_PARAM_IMAGEFX_CARTOON } }; - -static const int imagefx_map_size = sizeof(imagefx_map) / sizeof(imagefx_map[0]); - -static XREF_T metering_mode_map[] = { { "average", MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE }, - { "spot", MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT }, - { "backlit", MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT }, - { "matrix", MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX } }; - -static const int metering_mode_map_size = sizeof(metering_mode_map) / sizeof(metering_mode_map[0]); - -#define CommandSharpness 0 -#define CommandContrast 1 -#define CommandBrightness 2 -#define CommandSaturation 3 -#define CommandISO 4 -#define CommandVideoStab 5 -#define CommandEVComp 6 -#define CommandExposure 7 -#define CommandAWB 8 -#define CommandImageFX 9 -#define CommandColourFX 10 -#define CommandMeterMode 11 -#define CommandRotation 12 -#define CommandHFlip 13 -#define CommandVFlip 14 -#define CommandROI 15 - -#define parameter_reset -99999 - -/** - * Update the passed in parameter according to the rest of the parameters - * passed in. - * - * - * @return 0 if reached end of cycle for this parameter, !0 otherwise - */ -static int update_cycle_parameter(int* option, int min, int max, int increment) { - vcos_assert(option); - if (!option) - return 0; - - if (*option == parameter_reset) - *option = min - increment; - - *option += increment; - - if (*option > max) { - *option = parameter_reset; - return 0; - } else - return 1; -} - -/** - * Test/Demo code to cycle through a bunch of camera settings - * This code is pretty hacky so please don't complain!! - * It only does stuff that should have a visual impact (hence demo!) - * This will override any user supplied parameters - * - * Each call of this function will move on to the next setting - * - * @param camera Pointer to the camera to change settings on. - * @return 0 if reached end of complete sequence, !0 otherwise - */ - -int raspicamcontrol_cycle_test(MMAL_COMPONENT_T& camera) { - static int parameter = 0; - static int parameter_option = parameter_reset; // which value the parameter currently has - - // We are going to cycle through all the relevant entries in the parameter - // block and send options to the camera. - if (parameter == 0) { - // sharpness - if (update_cycle_parameter(¶meter_option, -100, 100, 10)) - raspicamcontrol_set_sharpness(camera, parameter_option); - else { - raspicamcontrol_set_sharpness(camera, 0); - parameter++; - } - } else if (parameter == 1) { - // contrast - if (update_cycle_parameter(¶meter_option, -100, 100, 10)) - raspicamcontrol_set_contrast(camera, parameter_option); - else { - raspicamcontrol_set_contrast(camera, 0); - parameter++; - } - } else if (parameter == 2) { - // brightness - if (update_cycle_parameter(¶meter_option, 0, 100, 10)) - raspicamcontrol_set_brightness(camera, parameter_option); - else { - raspicamcontrol_set_brightness(camera, 50); - parameter++; - } - } else if (parameter == 3) { - // contrast - if (update_cycle_parameter(¶meter_option, -100, 100, 10)) - raspicamcontrol_set_saturation(camera, parameter_option); - else { - parameter++; - raspicamcontrol_set_saturation(camera, 0); - } - } else if (parameter == 4) { - // EV - if (update_cycle_parameter(¶meter_option, -10, 10, 4)) - raspicamcontrol_set_exposure_compensation(camera, parameter_option); - else { - raspicamcontrol_set_exposure_compensation(camera, 0); - parameter++; - } - } else if (parameter == 5) { - // MMAL_PARAM_EXPOSUREMODE_T - if (update_cycle_parameter(¶meter_option, 0, exposure_map_size, 1)) - raspicamcontrol_set_exposure_mode(camera, (MMAL_PARAM_EXPOSUREMODE_T)exposure_map[parameter_option].mmal_mode); - else { - raspicamcontrol_set_exposure_mode(camera, MMAL_PARAM_EXPOSUREMODE_AUTO); - parameter++; - } - } else if (parameter == 6) { - // MMAL_PARAM_AWB_T - if (update_cycle_parameter(¶meter_option, 0, awb_map_size, 1)) - raspicamcontrol_set_awb_mode(camera, (MMAL_PARAM_AWBMODE_T)awb_map[parameter_option].mmal_mode); - else { - raspicamcontrol_set_awb_mode(camera, MMAL_PARAM_AWBMODE_AUTO); - parameter++; - } - } - if (parameter == 7) { - // MMAL_PARAM_IMAGEFX_T - if (update_cycle_parameter(¶meter_option, 0, imagefx_map_size, 1)) - raspicamcontrol_set_imageFX(camera, (MMAL_PARAM_IMAGEFX_T)imagefx_map[parameter_option].mmal_mode); - else { - raspicamcontrol_set_imageFX(camera, MMAL_PARAM_IMAGEFX_NONE); - parameter++; - } - } - if (parameter == 8) { - MMAL_PARAM_COLOURFX_T colfx = { 0, 0, 0 }; - switch (parameter_option) { - case parameter_reset: - parameter_option = 1; - colfx.u = 128; - colfx.v = 128; - break; - case 1: - parameter_option = 2; - colfx.u = 100; - colfx.v = 200; - break; - case 2: - parameter_option = parameter_reset; - colfx.enable = 0; - parameter++; - break; - } - raspicamcontrol_set_colourFX(camera, &colfx); - } - - // Orientation - if (parameter == 9) { - switch (parameter_option) { - case parameter_reset: - raspicamcontrol_set_rotation(camera, 90); - parameter_option = 1; - break; - - case 1: - raspicamcontrol_set_rotation(camera, 180); - parameter_option = 2; - break; - - case 2: - raspicamcontrol_set_rotation(camera, 270); - parameter_option = 3; - break; - - case 3: { - raspicamcontrol_set_rotation(camera, 0); - raspicamcontrol_set_flips(camera, 1, 0); - parameter_option = 4; - break; - } - case 4: { - raspicamcontrol_set_flips(camera, 0, 1); - parameter_option = 5; - break; - } - case 5: { - raspicamcontrol_set_flips(camera, 1, 1); - parameter_option = 6; - break; - } - case 6: { - raspicamcontrol_set_flips(camera, 0, 0); - parameter_option = parameter_reset; - parameter++; - break; - } - } - } - - if (parameter == 10) { - parameter = 1; - return 0; - } - - return 1; -} - -/** - * Function to take a string, a mapping, and return the int equivalent - * @param str Incoming string to match - * @param map Mapping data - * @param num_refs The number of items in the mapping data - * @return The integer match for the string, or -1 if no match - */ -static int map_xref(const char* str, const XREF_T* map, int num_refs) { - int i; - - for (i = 0; i < num_refs; i++) { - if (!strcasecmp(str, map[i].mode)) { - return map[i].mmal_mode; - } - } - return -1; -} - -/** - * Function to take a mmal enum (as int) and return the string equivalent - * @param en Incoming int to match - * @param map Mapping data - * @param num_refs The number of items in the mapping data - * @return const pointer to string, or NULL if no match - */ -static const char* unmap_xref(const int en, XREF_T* map, int num_refs) { - int i; - - for (i = 0; i < num_refs; i++) { - if (en == map[i].mmal_mode) { - return map[i].mode; - } - } - return NULL; -} - -/** - * Convert string to the MMAL parameter for exposure mode - * @param str Incoming string to match - * @return MMAL parameter matching the string, or the AUTO option if no match - * found - */ -/*static*/ MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char* str) { - int i = map_xref(str, exposure_map, exposure_map_size); - - if (i != -1) - return (MMAL_PARAM_EXPOSUREMODE_T)i; - - vcos_log_error("Unknown exposure mode: %s", str); - return MMAL_PARAM_EXPOSUREMODE_AUTO; -} - -/** - * Convert string to the MMAL parameter for AWB mode - * @param str Incoming string to match - * @return MMAL parameter matching the string, or the AUTO option if no match - * found - */ -MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char* str) { - int i = map_xref(str, awb_map, awb_map_size); - - if (i != -1) - return (MMAL_PARAM_AWBMODE_T)i; - - vcos_log_error("Unknown awb mode: %s", str); - return MMAL_PARAM_AWBMODE_AUTO; -} - -/** - * Convert string to the MMAL parameter for image effects mode - * @param str Incoming string to match - * @return MMAL parameter matching the strong, or the AUTO option if no match - * found - */ -MMAL_PARAM_IMAGEFX_T imagefx_mode_from_string(const char* str) { - int i = map_xref(str, imagefx_map, imagefx_map_size); - - if (i != -1) - return (MMAL_PARAM_IMAGEFX_T)i; - - vcos_log_error("Unknown image fx: %s", str); - return MMAL_PARAM_IMAGEFX_NONE; -} - -/** - * Convert string to the MMAL parameter for exposure metering mode - * @param str Incoming string to match - * @return MMAL parameter matching the string, or the AUTO option if no match - * found - */ -MMAL_PARAM_EXPOSUREMETERINGMODE_T metering_mode_from_string(const char* str) { - int i = map_xref(str, metering_mode_map, metering_mode_map_size); - - if (i != -1) - return (MMAL_PARAM_EXPOSUREMETERINGMODE_T)i; - - vcos_log_error("Unknown metering mode: %s", str); - return MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE; -} - -/** - * Dump contents of camera parameter structure to stdout for debugging/verbose - * logging - * - * @param params Const ref to parameters structure to dump - */ -void raspicamcontrol_dump_parameters(const RASPICAM_CAMERA_PARAMETERS& params) { - const char* exp_mode = unmap_xref(params.exposureMode, exposure_map, exposure_map_size); - const char* awb_mode = unmap_xref(params.awbMode, awb_map, awb_map_size); - const char* image_effect = unmap_xref(params.imageEffect, imagefx_map, imagefx_map_size); - const char* metering_mode = unmap_xref(params.exposureMeterMode, metering_mode_map, metering_mode_map_size); - - fprintf(stderr, "Sharpness %d, Contrast %d, Brightness %d\n", params.sharpness, params.contrast, params.brightness); - fprintf(stderr, "Saturation %d, ISO %d, Video Stabilisation %s, Exposure " - "compensation %d\n", - params.saturation, params.ISO, params.videoStabilisation ? "Yes" : "No", params.exposureCompensation); - fprintf(stderr, "Exposure Mode '%s', AWB Mode '%s', Image Effect '%s'\n", exp_mode, awb_mode, image_effect); - fprintf(stderr, "Metering Mode '%s', Colour Effect Enabled %s with U = %d, V = %d\n", metering_mode, - params.colourEffects.enable ? "Yes" : "No", params.colourEffects.u, params.colourEffects.v); - fprintf(stderr, "Rotation %d, hflip %s, vflip %s\n", params.rotation, params.hflip ? "Yes" : "No", - params.vflip ? "Yes" : "No"); - fprintf(stderr, "ROI x %lf, y %f, w %f h %f\n", params.roi.x, params.roi.y, params.roi.w, params.roi.h); -} - -/** - * Convert a MMAL status return value to a simple boolean of success - * ALso displays a fault if code is not success - * - * @param status The error code to convert - * @return 0 if status is sucess, 1 otherwise - */ -int mmal_status_to_int(MMAL_STATUS_T status) { - if (status == MMAL_SUCCESS) - return 0; - else { - switch (status) { - case MMAL_ENOMEM: - vcos_log_error("Out of memory"); - break; - case MMAL_ENOSPC: - vcos_log_error("Out of resources (other than memory)"); - break; - case MMAL_EINVAL: - vcos_log_error("Argument is invalid"); - break; - case MMAL_ENOSYS: - vcos_log_error("Function not implemented"); - break; - case MMAL_ENOENT: - vcos_log_error("No such file or directory"); - break; - case MMAL_ENXIO: - vcos_log_error("No such device or address"); - break; - case MMAL_EIO: - vcos_log_error("I/O error"); - break; - case MMAL_ESPIPE: - vcos_log_error("Illegal seek"); - break; - case MMAL_ECORRUPT: - vcos_log_error("Data is corrupt \attention FIXME: not POSIX"); - break; - case MMAL_ENOTREADY: - vcos_log_error("Component is not ready \attention FIXME: not POSIX"); - break; - case MMAL_ECONFIG: - vcos_log_error("Component is not configured \attention FIXME: not POSIX"); - break; - case MMAL_EISCONN: - vcos_log_error("Port is already connected "); - break; - case MMAL_ENOTCONN: - vcos_log_error("Port is disconnected"); - break; - case MMAL_EAGAIN: - vcos_log_error("Resource temporarily unavailable. Try again later"); - break; - case MMAL_EFAULT: - vcos_log_error("Bad address"); - break; - default: - vcos_log_error("Unknown status error"); - break; - } - - return 1; - } -} - -/** - * Give the supplied parameter block a set of default values - * @params Pointer to parameter block - */ -void raspicamcontrol_set_defaults(RASPICAM_CAMERA_PARAMETERS& params) { - params.sharpness = 0; - params.contrast = 0; - params.brightness = 50; - params.saturation = 0; - params.ISO = 400; - params.videoStabilisation = 0; - params.exposureCompensation = 0; - params.exposureMode = MMAL_PARAM_EXPOSUREMODE_AUTO; - params.exposureMeterMode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE; - params.awbMode = MMAL_PARAM_AWBMODE_AUTO; - // params.imageEffect = MMAL_PARAM_IMAGEFX_SATURATION; - params.imageEffect = MMAL_PARAM_IMAGEFX_NONE; - params.colourEffects.enable = 0; - params.colourEffects.u = 128; - params.colourEffects.v = 128; - params.rotation = 0; - params.hflip = params.vflip = 0; - params.roi.x = params.roi.y = 0.0; - params.roi.w = params.roi.h = 1.0; - params.shutter_speed = 0; // automatic -} - -/** - * Set the specified camera to all the specified settings - * @param camera Pointer to camera component - * @param params refrence to parameter block containing parameters - * @return 0 if successful, none-zero if unsuccessful. - */ -int raspicamcontrol_set_all_parameters(MMAL_COMPONENT_T& camera, const RASPICAM_CAMERA_PARAMETERS& params) { - int result; - - result = raspicamcontrol_set_saturation(camera, params.saturation); - result += raspicamcontrol_set_sharpness(camera, params.sharpness); - result += raspicamcontrol_set_contrast(camera, params.contrast); - result += raspicamcontrol_set_brightness(camera, params.brightness); - result += raspicamcontrol_set_ISO(camera, params.ISO); - result += raspicamcontrol_set_video_stabilisation(camera, params.videoStabilisation); - result += raspicamcontrol_set_exposure_compensation(camera, params.exposureCompensation); - result += raspicamcontrol_set_exposure_mode(camera, params.exposureMode); - result += raspicamcontrol_set_metering_mode(camera, params.exposureMeterMode); - result += raspicamcontrol_set_awb_mode(camera, params.awbMode); - result += raspicamcontrol_set_imageFX(camera, params.imageEffect); - result += raspicamcontrol_set_colourFX(camera, ¶ms.colourEffects); - // result += raspicamcontrol_set_thumbnail_parameters(camera, - // ¶ms.thumbnailConfig); TODO Not working for some reason - result += raspicamcontrol_set_rotation(camera, params.rotation); - result += raspicamcontrol_set_flips(camera, params.hflip, params.vflip); - result += raspicamcontrol_set_ROI(camera, params.roi); - result += raspicamcontrol_set_shutter_speed(camera, params.shutter_speed); - return result; -} - -/** - * Adjust the saturation level for images - * @param camera Pointer to camera component - * @param saturation Value to adjust, -100 to 100 - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_saturation(MMAL_COMPONENT_T& camera, int saturation) { - int ret = 0; - - if (saturation >= -100 && saturation <= 100) { - MMAL_RATIONAL_T value = { saturation, 100 }; - ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_SATURATION, value)); - } else { - vcos_log_error("Invalid saturation value"); - ret = 1; - } - - return ret; -} - -/** - * Set the sharpness of the image - * @param camera Pointer to camera component - * @param sharpness Sharpness adjustment -100 to 100 - */ -int raspicamcontrol_set_sharpness(MMAL_COMPONENT_T& camera, int sharpness) { - int ret = 0; - - if (sharpness >= -100 && sharpness <= 100) { - MMAL_RATIONAL_T value = { sharpness, 100 }; - ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_SHARPNESS, value)); - } else { - vcos_log_error("Invalid sharpness value"); - ret = 1; - } - - return ret; -} - -/** - * Set the contrast adjustment for the image - * @param camera Pointer to camera component - * @param contrast Contrast adjustment -100 to 100 - * @return - */ -int raspicamcontrol_set_contrast(MMAL_COMPONENT_T& camera, int contrast) { - int ret = 0; - - if (contrast >= -100 && contrast <= 100) { - MMAL_RATIONAL_T value = { contrast, 100 }; - ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_CONTRAST, value)); - } else { - vcos_log_error("Invalid contrast value"); - ret = 1; - } - - return ret; -} - -/** - * Adjust the brightness level for images - * @param camera Pointer to camera component - * @param brightness Value to adjust, 0 to 100 - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_brightness(MMAL_COMPONENT_T& camera, int brightness) { - int ret = 0; - - if (brightness >= 0 && brightness <= 100) { - MMAL_RATIONAL_T value = { brightness, 100 }; - ret = mmal_status_to_int(mmal_port_parameter_set_rational(camera.control, MMAL_PARAMETER_BRIGHTNESS, value)); - } else { - vcos_log_error("Invalid brightness value"); - ret = 1; - } - - return ret; -} - -/** - * Adjust the ISO used for images - * @param camera Pointer to camera component - * @param ISO Value to set TODO : - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_ISO(MMAL_COMPONENT_T& camera, int ISO) { - return mmal_status_to_int(mmal_port_parameter_set_uint32(camera.control, MMAL_PARAMETER_ISO, ISO)); -} - -/** - * Adjust the metering mode for images - * @param camera Pointer to camera component - * @param saturation Value from following - * - MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE, - * - MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT, - * - MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT, - * - MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_metering_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMETERINGMODE_T m_mode) { - MMAL_PARAMETER_EXPOSUREMETERINGMODE_T meter_mode = { { MMAL_PARAMETER_EXP_METERING_MODE, sizeof(meter_mode) }, - m_mode }; - - return mmal_status_to_int(mmal_port_parameter_set(camera.control, &meter_mode.hdr)); -} - -/** - * Set the video stabilisation flag. Only used in video mode - * @param camera Pointer to camera component - * @param saturation Flag 0 off 1 on - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_video_stabilisation(MMAL_COMPONENT_T& camera, int vstabilisation) { - return mmal_status_to_int( - mmal_port_parameter_set_boolean(camera.control, MMAL_PARAMETER_VIDEO_STABILISATION, vstabilisation)); -} - -/** - * Adjust the exposure compensation for images (EV) - * @param camera Pointer to camera component - * @param exp_comp Value to adjust, -10 to +10 - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_exposure_compensation(MMAL_COMPONENT_T& camera, int exp_comp) { - return mmal_status_to_int(mmal_port_parameter_set_int32(camera.control, MMAL_PARAMETER_EXPOSURE_COMP, exp_comp)); -} - -/** - * Set exposure mode for images - * @param camera Pointer to camera component - * @param mode Exposure mode to set from - * - MMAL_PARAM_EXPOSUREMODE_OFF, - * - MMAL_PARAM_EXPOSUREMODE_AUTO, - * - MMAL_PARAM_EXPOSUREMODE_NIGHT, - * - MMAL_PARAM_EXPOSUREMODE_NIGHTPREVIEW, - * - MMAL_PARAM_EXPOSUREMODE_BACKLIGHT, - * - MMAL_PARAM_EXPOSUREMODE_SPOTLIGHT, - * - MMAL_PARAM_EXPOSUREMODE_SPORTS, - * - MMAL_PARAM_EXPOSUREMODE_SNOW, - * - MMAL_PARAM_EXPOSUREMODE_BEACH, - * - MMAL_PARAM_EXPOSUREMODE_VERYLONG, - * - MMAL_PARAM_EXPOSUREMODE_FIXEDFPS, - * - MMAL_PARAM_EXPOSUREMODE_ANTISHAKE, - * - MMAL_PARAM_EXPOSUREMODE_FIREWORKS, - * - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_exposure_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_EXPOSUREMODE_T mode) { - MMAL_PARAMETER_EXPOSUREMODE_T exp_mode = { { MMAL_PARAMETER_EXPOSURE_MODE, sizeof(exp_mode) }, mode }; - - return mmal_status_to_int(mmal_port_parameter_set(camera.control, &exp_mode.hdr)); -} - -/** - * Set the aWB (auto white balance) mode for images - * @param camera Pointer to camera component - * @param awb_mode Value to set from - * - MMAL_PARAM_AWBMODE_OFF, - * - MMAL_PARAM_AWBMODE_AUTO, - * - MMAL_PARAM_AWBMODE_SUNLIGHT, - * - MMAL_PARAM_AWBMODE_CLOUDY, - * - MMAL_PARAM_AWBMODE_SHADE, - * - MMAL_PARAM_AWBMODE_TUNGSTEN, - * - MMAL_PARAM_AWBMODE_FLUORESCENT, - * - MMAL_PARAM_AWBMODE_INCANDESCENT, - * - MMAL_PARAM_AWBMODE_FLASH, - * - MMAL_PARAM_AWBMODE_HORIZON, - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_awb_mode(MMAL_COMPONENT_T& camera, MMAL_PARAM_AWBMODE_T awb_mode) { - MMAL_PARAMETER_AWBMODE_T param = { { MMAL_PARAMETER_AWB_MODE, sizeof(param) }, awb_mode }; - - return mmal_status_to_int(mmal_port_parameter_set(camera.control, ¶m.hdr)); -} - -/** - * Set the image effect for the images - * @param camera Pointer to camera component - * @param imageFX Value from - * - MMAL_PARAM_IMAGEFX_NONE, - * - MMAL_PARAM_IMAGEFX_NEGATIVE, - * - MMAL_PARAM_IMAGEFX_SOLARIZE, - * - MMAL_PARAM_IMAGEFX_POSTERIZE, - * - MMAL_PARAM_IMAGEFX_WHITEBOARD, - * - MMAL_PARAM_IMAGEFX_BLACKBOARD, - * - MMAL_PARAM_IMAGEFX_SKETCH, - * - MMAL_PARAM_IMAGEFX_DENOISE, - * - MMAL_PARAM_IMAGEFX_EMBOSS, - * - MMAL_PARAM_IMAGEFX_OILPAINT, - * - MMAL_PARAM_IMAGEFX_HATCH, - * - MMAL_PARAM_IMAGEFX_GPEN, - * - MMAL_PARAM_IMAGEFX_PASTEL, - * - MMAL_PARAM_IMAGEFX_WATERCOLOUR, - * - MMAL_PARAM_IMAGEFX_FILM, - * - MMAL_PARAM_IMAGEFX_BLUR, - * - MMAL_PARAM_IMAGEFX_SATURATION, - * - MMAL_PARAM_IMAGEFX_COLOURSWAP, - * - MMAL_PARAM_IMAGEFX_WASHEDOUT, - * - MMAL_PARAM_IMAGEFX_POSTERISE, - * - MMAL_PARAM_IMAGEFX_COLOURPOINT, - * - MMAL_PARAM_IMAGEFX_COLOURBALANCE, - * - MMAL_PARAM_IMAGEFX_CARTOON, - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_imageFX(MMAL_COMPONENT_T& camera, MMAL_PARAM_IMAGEFX_T imageFX) { - MMAL_PARAMETER_IMAGEFX_T imgFX = { { MMAL_PARAMETER_IMAGE_EFFECT, sizeof(imgFX) }, imageFX }; - - return mmal_status_to_int(mmal_port_parameter_set(camera.control, &imgFX.hdr)); -} - -/* TODO :what to do with the image effects parameters? - MMAL_PARAMETER_IMAGEFX_PARAMETERS_T imfx_param = -{{MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,sizeof(imfx_param)}, imageFX, 0, {0}}; -mmal_port_parameter_set(camera.control, &imfx_param.hdr); - */ - -/** - * Set the colour effect for images (Set UV component) - * @param camera Pointer to camera component - * @param colourFX Contains enable state and U and V numbers to set (e.g. - * 128,128 = Black and white) - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_colourFX(MMAL_COMPONENT_T& camera, const MMAL_PARAM_COLOURFX_T* colourFX) { - MMAL_PARAMETER_COLOURFX_T colfx = { { MMAL_PARAMETER_COLOUR_EFFECT, sizeof(colfx) }, 0, 0, 0 }; - - colfx.enable = colourFX->enable; - colfx.u = colourFX->u; - colfx.v = colourFX->v; - - return mmal_status_to_int(mmal_port_parameter_set(camera.control, &colfx.hdr)); -} - -/** - * Set the rotation of the image - * @param camera Pointer to camera component - * @param rotation Degree of rotation (any number, but will be converted to - * 0,90,180 or 270 only) - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_rotation(MMAL_COMPONENT_T& camera, int rotation) { - int ret; - int my_rotation = ((rotation % 360) / 90) * 90; - - ret = mmal_port_parameter_set_int32(camera.output[0], MMAL_PARAMETER_ROTATION, my_rotation); - mmal_port_parameter_set_int32(camera.output[1], MMAL_PARAMETER_ROTATION, my_rotation); - mmal_port_parameter_set_int32(camera.output[2], MMAL_PARAMETER_ROTATION, my_rotation); - - return ret; -} - -/** - * Set the flips state of the image - * @param camera Pointer to camera component - * @param hflip If true, horizontally flip the image - * @param vflip If true, vertically flip the image - * - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_flips(MMAL_COMPONENT_T& camera, int hflip, int vflip) { - MMAL_PARAMETER_MIRROR_T mirror = { { MMAL_PARAMETER_MIRROR, sizeof(MMAL_PARAMETER_MIRROR_T) }, - MMAL_PARAM_MIRROR_NONE }; - - if (hflip && vflip) - mirror.value = MMAL_PARAM_MIRROR_BOTH; - else if (hflip) - mirror.value = MMAL_PARAM_MIRROR_HORIZONTAL; - else if (vflip) - mirror.value = MMAL_PARAM_MIRROR_VERTICAL; - - mmal_port_parameter_set(camera.output[0], &mirror.hdr); - mmal_port_parameter_set(camera.output[1], &mirror.hdr); - return mmal_port_parameter_set(camera.output[2], &mirror.hdr); -} - -/** - * Set the ROI of the sensor to use for captures/preview - * @param camera Pointer to camera component - * @param rect Normalised coordinates of ROI rectangle - * - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_ROI(MMAL_COMPONENT_T& camera, PARAM_FLOAT_RECT_T rect) { - MMAL_PARAMETER_INPUT_CROP_T crop = { { MMAL_PARAMETER_INPUT_CROP, sizeof(MMAL_PARAMETER_INPUT_CROP_T) } }; - - crop.rect.x = (65536 * rect.x); - crop.rect.y = (65536 * rect.y); - crop.rect.width = (65536 * rect.w); - crop.rect.height = (65536 * rect.h); - - return mmal_port_parameter_set(camera.control, &crop.hdr); -} - -/** - * Set the shuuter_speed of the sensor - * @param camera Pointer to camera component - * @param the shutter_speed in microseconds - * @return 0 if successful, non-zero if any parameters out of range - */ -int raspicamcontrol_set_shutter_speed(MMAL_COMPONENT_T& camera, int shutter_speed) { - return mmal_status_to_int( - mmal_port_parameter_set_uint32(camera.control, MMAL_PARAMETER_SHUTTER_SPEED, shutter_speed)); -} - -/** - * Asked GPU how much memory it has allocated - * - * @return amount of memory in MB - */ -/*static int raspicamcontrol_get_mem_gpu(void) -{ - char response[80] = ""; - int gpu_mem = 0; - if (vc_gencmd(response, sizeof response, "get_mem gpu") == 0) - vc_gencmd_number_property(response, "gpu", &gpu_mem); - return gpu_mem; -}*/ - -/** - * Ask GPU about its camera abilities - * @param supported None-zero if software supports the camera - * @param detected None-zero if a camera has been detected - */ -/*static void raspicamcontrol_get_camera(int *supported, int *detected) -{ - char response[80] = ""; - if (vc_gencmd(response, sizeof response, "get_camera") == 0) - { - if (supported) - vc_gencmd_number_property(response, "supported", supported); - if (detected) - vc_gencmd_number_property(response, "detected", detected); - } -}*/ - -/** - * Check to see if camera is supported, and we have allocated enough meooryAsk - * GPU about its camera abilities - * @param supported None-zero if software supports the camera - * @param detected None-zero if a camera has been detected - */ -void raspicamcontrol_check_configuration(int min_gpu_mem) { - int gpu_mem = 256; // raspicamcontrol_get_mem_gpu(); - int supported = 1, detected = 1; - // raspicamcontrol_get_camera(&supported, &detected); - if (!supported) - vcos_log_error("Camera is not enabled in this build. Try running \"sudo " - "raspi-config\" and ensure that \"camera\" has been enabled\n"); - else if (gpu_mem < min_gpu_mem) - vcos_log_error("Only %dM of gpu_mem is configured. Try running \"sudo " - "raspi-config\" and ensure that \"memory_split\" has a value of %d " - "or greater\n", - gpu_mem, min_gpu_mem); - else if (!detected) - vcos_log_error("Camera is not detected. Please check carefully the camera module " - "is installed correctly\n"); - else - vcos_log_error("Failed to run camera app. Please check for firmware updates\n"); -} - -#endif // __arm__ From e1c73e25dee76cc9fa23a11215f948f114468443 Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 02:01:35 +0400 Subject: [PATCH 09/18] description of 'stereo_node' was updated --- README.md | 54 +++++++++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 53 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index f18254a..cdcbcf3 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ rosdep install --from-paths src --ignore-src --rosdistro=kinetic -y Compile the code with `catkin_make`. -## Running the Node +## Running the monocamera Node Once you have the node built, you can run it using a launch file. For a V2.x camera, run `roslaunch raspicam_node camerav2_1280x960.launch` @@ -44,6 +44,13 @@ For a V1.x camera, run `roslaunch raspicam_node camerav1_1280x720.launch` Use `rqt_image_view` on a connected computer to view the published image. +## Running the stereocamera Node +Once you have the node built, you can run it using a launch file. + +For a V2.x camera, run `roslaunch raspicam_node stereo_1280x720.launch` + +Use `rqt_image_view` on a connected computer to view the published image. + ## Configuring the node with dynamic reconfigure The `raspicam_node` supports dynamically reconfiguring the camera parameters. @@ -68,6 +75,7 @@ make sure that the camera cable is properly seated on both ends, and that the ca ## Node Information +# Raspicam_node Topics: * `~/image/compressed`: @@ -110,6 +118,50 @@ This parameter is mainly present in order to keep backward compatibility. * `~enable_imv` (bool): Publish inline motion vectors computed by the GPU +* `~camera_id` (int): The camera id (only supported on Compute Module) +# Stereo_node + +Topics: + +* `~/image_left`: + Publishes `sensor_msgs/Image` from the camera module. + +* `~/image_right`: + Publishes `sensor_msgs/Image` from the camera module. + +* `~/left_camera_info`: + Publishes `sensor_msgs/CameraInfo` left camera info for each frame. + +* `~/left_camera_info`: + Publishes `sensor_msgs/CameraInfo` right camera info for each frame. + +Services: + +* `~/set_camera_info`: Used to update calibration info for the camera. + +Parameters: + +* `~private_topics` (bool): By default the topics are private, meaning the node name will be added in front of every topic name. +If you don't want the topics to be private, you can set this parameter to "true". +This parameter is mainly present in order to keep backward compatibility. + +* `~camera_frame_id` (tf frame): The frame identifier to associate the camera. + +* `~left_camera_info_url`: The URL of the camera calibration `.yaml` file for left camera. +* `~left_camera_info_url`: The URL of the camera calibration `.yaml` file for right camera. + +* `~left_camera_name` (string): The name of the left camera, should match with name in camera_info file. + +* `~right_camera_name` (string): The name of the right camera, should match with name in camera_info file. + +* `~framerate` (fps): Framerate to capture at. Maximum 90fps + +* `~height` (pixels): Height to capture images at. + +* `~width` (pixels): Width to capture images at. + +* `~quality` (0-100): Quality of the captured images. + * `~camera_id` (int): The camera id (only supported on Compute Module) ## Calibration From 5eb02df8624ee3df4bd06ac1c8e26d24874fac87 Mon Sep 17 00:00:00 2001 From: Andrew Lashchev Date: Sun, 10 Nov 2019 23:21:21 +0400 Subject: [PATCH 10/18] fix stereo mode in 'raspicam_node' --- src/raspicam_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/raspicam_node.cpp b/src/raspicam_node.cpp index 0edcc01..f4f5f56 100644 --- a/src/raspicam_node.cpp +++ b/src/raspicam_node.cpp @@ -470,7 +470,7 @@ static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) { goto error; } - state.camera_parameters.stereo_mode.mode = stereo_mode_from_string("tb"); + // state.camera_parameters.stereo_mode.mode = stereo_mode_from_string("tb"); status = static_cast(raspicamcontrol_set_stereo_mode(camera->output[0], &state.camera_parameters.stereo_mode)); if (status != MMAL_SUCCESS) { From 41c33884475521c1ba97681c725cdb2504e57fa4 Mon Sep 17 00:00:00 2001 From: lashhev Date: Wed, 12 Feb 2020 19:58:39 +0300 Subject: [PATCH 11/18] use opencv and some optimization flags --- CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f38de7..e2c02da 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,7 @@ set(CMAKE_CXX_STANDARD 14) # use C++14 # Enable compile optimizations # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") set(CMAKE_BUILD_TYPE Release) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=armv8-a+crc -mtune=cortex-a53 -mfpu=crypto-neon-fp-armv8 -mfloat-abi=hard -ftree-vectorize -funsafe-math-optimizations -O3 -pipe") # add_definitions(-D_DEBUG) ## Find catkin macros and libraries @@ -20,8 +21,9 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure diagnostic_updater message_generation + cv_bridge ) - +find_package(OpenCV 4.2 REQUIRED COMPONENTS imgproc highgui) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -122,6 +124,7 @@ include_directories(include /opt/vc/include /opt/vc/include/interface/vcos/pthreads /opt/vc/include/interface/vmcs_host/linux + ${OpenCV_INCLUDE_DIRS} ) add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral) @@ -158,6 +161,7 @@ target_link_libraries(raspicam_node raspicli ${catkin_LIBRARIES} ${RPI_LIBRARIES} + ${OpenCV_LIBS} ) ############# From 4fe9be39c6d644b521b950285e0a2c3b7f512dd9 Mon Sep 17 00:00:00 2001 From: lashhev Date: Wed, 12 Feb 2020 20:00:25 +0300 Subject: [PATCH 12/18] logging improvements --- src/RaspiCamControl.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/RaspiCamControl.c b/src/RaspiCamControl.c index 40ddad6..9cabc32 100755 --- a/src/RaspiCamControl.c +++ b/src/RaspiCamControl.c @@ -82,7 +82,7 @@ static const int awb_map_size = sizeof(awb_map) / sizeof(awb_map[0]); static XREF_T imagefx_map[] = { {"none", MMAL_PARAM_IMAGEFX_NONE}, - {"negative", MMAL_PARAM_IMAGEFX_NEGATIVE}, + {"negative", MMAL_PARAM_IMAGEFX_NEGATIVE}, {"solarise", MMAL_PARAM_IMAGEFX_SOLARIZE}, {"sketch", MMAL_PARAM_IMAGEFX_SKETCH}, {"denoise", MMAL_PARAM_IMAGEFX_DENOISE}, From f21ef4337fcd197dfff37bcd2ff74ccab133666e Mon Sep 17 00:00:00 2001 From: lashhev Date: Wed, 12 Feb 2020 20:00:41 +0300 Subject: [PATCH 13/18] logging improvements --- src/RaspiCamControl.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/RaspiCamControl.c b/src/RaspiCamControl.c index 9cabc32..55fcb0d 100755 --- a/src/RaspiCamControl.c +++ b/src/RaspiCamControl.c @@ -851,22 +851,22 @@ int mmal_status_to_int(MMAL_STATUS_T status) { switch (status) { - case MMAL_ENOMEM : vcos_log_error("Out of memory"); break; - case MMAL_ENOSPC : vcos_log_error("Out of resources (other than memory)"); break; - case MMAL_EINVAL: vcos_log_error("Argument is invalid"); break; - case MMAL_ENOSYS : vcos_log_error("Function not implemented"); break; - case MMAL_ENOENT : vcos_log_error("No such file or directory"); break; - case MMAL_ENXIO : vcos_log_error("No such device or address"); break; - case MMAL_EIO : vcos_log_error("I/O error"); break; - case MMAL_ESPIPE : vcos_log_error("Illegal seek"); break; - case MMAL_ECORRUPT : vcos_log_error("Data is corrupt \attention FIXME: not POSIX"); break; - case MMAL_ENOTREADY :vcos_log_error("Component is not ready \attention FIXME: not POSIX"); break; - case MMAL_ECONFIG : vcos_log_error("Component is not configured \attention FIXME: not POSIX"); break; - case MMAL_EISCONN : vcos_log_error("Port is already connected "); break; - case MMAL_ENOTCONN : vcos_log_error("Port is disconnected"); break; - case MMAL_EAGAIN : vcos_log_error("Resource temporarily unavailable. Try again later"); break; - case MMAL_EFAULT : vcos_log_error("Bad address"); break; - default : vcos_log_error("Unknown status error"); break; + case MMAL_ENOMEM : fprintf(stderr, "Out of memory"); vcos_log_error("Out of memory"); break; + case MMAL_ENOSPC : fprintf(stderr, "Out of resources (other than memory)"); vcos_log_error("Out of resources (other than memory)"); break; + case MMAL_EINVAL: fprintf(stderr, "Argument is invalid"); vcos_log_error("Argument is invalid"); break; + case MMAL_ENOSYS : fprintf(stderr, "Function not implemented"); vcos_log_error("Function not implemented"); break; + case MMAL_ENOENT : fprintf(stderr, "No such file or directory"); vcos_log_error("No such file or directory"); break; + case MMAL_ENXIO : fprintf(stderr, "No such device or address"); vcos_log_error("No such device or address"); break; + case MMAL_EIO : fprintf(stderr, "I/O error"); vcos_log_error("I/O error"); break; + case MMAL_ESPIPE : fprintf(stderr, "Illegal seek"); vcos_log_error("Illegal seek"); break; + case MMAL_ECORRUPT : fprintf(stderr, "Data is corrupt \attention FIXME: not POSIX"); vcos_log_error("Data is corrupt \attention FIXME: not POSIX"); break; + case MMAL_ENOTREADY : fprintf(stderr, "Component is not ready \attention FIXME: not POSIX"); vcos_log_error("Component is not ready \attention FIXME: not POSIX"); break; + case MMAL_ECONFIG : fprintf(stderr, "Component is not ready \attention FIXME: not POSIX"); vcos_log_error("Component is not configured \attention FIXME: not POSIX"); break; + case MMAL_EISCONN : fprintf(stderr, "Port is already connected "); vcos_log_error("Port is already connected "); break; + case MMAL_ENOTCONN : fprintf(stderr, "Port is disconnected"); vcos_log_error("Port is disconnected"); break; + case MMAL_EAGAIN : fprintf(stderr, "Resource temporarily unavailable. Try again later"); vcos_log_error("Resource temporarily unavailable. Try again later"); break; + case MMAL_EFAULT : fprintf(stderr, "Bad address"); vcos_log_error("Bad address"); break; + default : fprintf(stderr, "Unknown status error"); vcos_log_error("Unknown status error"); break; } return 1; From 6bc85f2c2cf068a83643550cfc01eca4b6e80d6a Mon Sep 17 00:00:00 2001 From: lashhev Date: Wed, 12 Feb 2020 20:01:56 +0300 Subject: [PATCH 14/18] some new parameters were added --- launch/stereo_1280x720.launch | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/launch/stereo_1280x720.launch b/launch/stereo_1280x720.launch index 585ce3a..61cbd22 100644 --- a/launch/stereo_1280x720.launch +++ b/launch/stereo_1280x720.launch @@ -1,12 +1,15 @@ - + - + - + + + @@ -14,10 +17,12 @@ - - - - - + + + + + + \ No newline at end of file From 654b0cfdd88f7f1da5dac5a0c1a514d9c4d7a157 Mon Sep 17 00:00:00 2001 From: lashhev Date: Wed, 12 Feb 2020 20:03:02 +0300 Subject: [PATCH 15/18] resize image option was add --- include/RaspiCamControl.h | 1 + src/stereo_node.cpp | 188 ++++++++++++++++++++++++-------------- 2 files changed, 119 insertions(+), 70 deletions(-) diff --git a/include/RaspiCamControl.h b/include/RaspiCamControl.h index 55cbc63..535c678 100644 --- a/include/RaspiCamControl.h +++ b/include/RaspiCamControl.h @@ -166,6 +166,7 @@ typedef enum { MMAL_PARAM_EXPOSUREMODE_T exposure_mode_from_string(const char *str); MMAL_PARAM_AWBMODE_T awb_mode_from_string(const char *str); MMAL_STEREOSCOPIC_MODE_T stereo_mode_from_string(const char *str); +MMAL_PARAM_IMAGEFX_T imagefx_mode_from_string(const char *str); void raspicamcontrol_check_configuration(int min_gpu_mem); int raspicamcontrol_parse_cmdline(RASPICAM_CAMERA_PARAMETERS *params, const char *arg1, const char *arg2); diff --git a/src/stereo_node.cpp b/src/stereo_node.cpp index ccbcc36..675a437 100644 --- a/src/stereo_node.cpp +++ b/src/stereo_node.cpp @@ -38,8 +38,19 @@ int main(int argc, char** argv) { #endif // __x86_64__ -#ifdef __arm__ - +// #ifdef __arm__ +#define SHUTDOWN(reason) \ + ROS_ERROR(reason); \ + close_cam(state); \ + ros::shutdown(); + +#define IF_FALSE_SHUTDOWN(statement, reason) \ + if(statement == 1) \ + { \ + SHUTDOWN(reason) \ + return; \ + } + // We use some GNU extensions (basename) #include #include @@ -68,8 +79,9 @@ int main(int argc, char** argv) { #include "sensor_msgs/CompressedImage.h" #include "sensor_msgs/Image.h" #include "sensor_msgs/SetCameraInfo.h" +#include #include "std_srvs/Empty.h" - #include "raspicam_node/MotionVectors.h" +// #include "raspicam_node/MotionVectors.h" #include #include extern "C" { @@ -79,6 +91,9 @@ extern "C" { #include #include "mmal_cxx_helper.h" +#include +#include +#include static constexpr int IMG_BUFFER_SIZE = 10 * 1024 * 1024; // 10 MB @@ -99,10 +114,13 @@ struct RASPIVID_STATE { bool isInit; int width; /// Requested width of image int height; /// requested height of image + int new_width; + int new_height; int framerate; /// Requested frame rate (fps) int quality; - int camera_id = 0; + std::string encoding; + std::string imagefx_mode; RASPICAM_CAMERA_PARAMETERS camera_parameters; /// Camera setup parameters @@ -152,23 +170,77 @@ sensor_msgs::CameraInfo right_info; std::string camera_frame_id; int skip_frames = 0; -/** - * Assign a default set of parameters to the state passed in - * - * @param state state structure to assign defaults to - * @param nh NodeHandle to get params from - */ +bool resize(sensor_msgs::Image& source_msg, + sensor_msgs::Image& resized_msg, cv::Size size, std::string encoding) +{ + // ROS_WARN("Starting resizing ..."); + cv_bridge::CvImagePtr bridge = cv_bridge::toCvCopy(source_msg, encoding); + cv::resize(bridge->image, bridge->image, size, 0, 0, cv::INTER_LINEAR); + // cv::imshow("resized", image); + // cv::waitKey(1); + resized_msg = *bridge->toImageMsg().get(); + // ROS_WARN("Resizing finished..."); + return true; +} + void print_image(const sensor_msgs::Image& msg) { std::cout << "header { stamp: { sec: " << msg.header.stamp.sec << ", nsec: " << msg.header.stamp.nsec << ", frameid: " << msg.header.frame_id << " }, \nheight: " << msg.height << "\nwidth: " << msg.width << "\nstep: " << msg.step << "\nencoding: " << msg.encoding << "\ndata_size: " << msg.data.size() << std::endl; } + +int close_cam(RASPIVID_STATE& state) { + if (state.isInit) { + state.isInit = false; + MMAL_COMPONENT_T* camera = state.camera_component.get(); + MMAL_COMPONENT_T* splitter = state.splitter_component.get(); + + // Destroy splitter port connection + state.splitter_connection.reset(nullptr); + + + // Destroy splitter component + if (splitter) { + // Get rid of any port buffers first + state.splitter_pool.reset(nullptr); + // Delete callback structure + if (splitter->output[1]->userdata) { + delete splitter->output[1]->userdata; + } + state.splitter_component.reset(nullptr); + } + + // destroy camera component + if (camera) { + state.camera_component.reset(nullptr); + } + ROS_INFO("Video capture stopped\n"); + return 0; + } else + return 1; +} + static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { - nh.param("width", state.width, 1280); - nh.param("height", state.height, 720); - state.height*=2; - nh.param("quality", state.quality, 80); + nh.param("imagefx_mode", state.imagefx_mode, "denoise"); + nh.param("encoding", state.encoding, "bgr8"); + if(state.encoding == "mono8") + state.encoding = sensor_msgs::image_encodings::MONO8; + else if(state.encoding == "bgr8") + { + state.encoding = sensor_msgs::image_encodings::BGR8; + } + else + { + ROS_WARN("Unknown encoding. Set to default"); + state.encoding = sensor_msgs::image_encodings::BGR8; + } + + nh.param("width", state.new_width, 1280); + nh.param("height", state.new_height, 720); + state.width = 1280; + state.height = 720*2; + nh.param("quality", state.quality, 100); if (state.quality < 0 && state.quality > 100) { ROS_WARN("quality: %d is outside valid range 0-100, defaulting to 80", state.quality); state.quality = 80; @@ -185,7 +257,7 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { // Set up the camera_parameters to default raspicamcontrol_set_defaults(&state.camera_parameters); - + state.camera_parameters.imageEffect = imagefx_mode_from_string(state.imagefx_mode.c_str()); bool temp; nh.param("hFlip", temp, false); state.camera_parameters.hflip = temp; // Hack for bool param => int variable @@ -204,10 +276,10 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { * @param port Pointer to port from which callback originated * @param buffer mmal buffer header pointer */ -void update_image(sensor_msgs::Image& msg, MMAL_BUFFER_HEADER_T* buffer, PORT_USERDATA* pData) +void update_image(sensor_msgs::Image& msg, MMAL_BUFFER_HEADER_T* buffer, PORT_USERDATA* pData, ros::Time ts) { msg.header.frame_id = camera_frame_id; - msg.header.stamp = ros::Time::now(); //(double(buffer->dts)/1e6); + msg.header.stamp = ts; //(double(buffer->dts)/1e6); msg.encoding = "bgr8"; // #ifdef _DEBUG // DBG_MSG("//DEBUG// - buffer->dts:") @@ -221,6 +293,7 @@ void update_image(sensor_msgs::Image& msg, MMAL_BUFFER_HEADER_T* buffer, PORT_US } static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* buffer) { // We pass our file handle and other stuff in via the userdata field. + auto ts = ros::Time::now(); PORT_USERDATA* pData = port->userdata; if (pData && pData->pstate.isInit) { size_t bytes_written = buffer->length; @@ -257,8 +330,9 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu pData->frames_skipped++; } else { pData->frames_skipped = 0; - update_image(left_image.msg, buffer, pData); - update_image(right_image.msg, buffer, pData); + + update_image(left_image.msg, buffer, pData, ts); + update_image(right_image.msg, buffer, pData, ts); auto start = pData->buffer[pData->frame & 1].get(); auto end = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); auto start1 = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); @@ -282,8 +356,14 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu print_image(left_image.msg); print_image(right_image.msg); #endif // _DEBUG - + // ROS_WARN("Publishing images starting..."); + sensor_msgs::Image image_msg; + resize(left_image.msg, image_msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); + // ROS_WARN("Publishing left image starting..."); + left_image.msg = image_msg; left_image.pub->publish(left_image.msg); + resize(right_image.msg, image_msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); + right_image.msg = image_msg; right_image.pub->publish(right_image.msg); #ifdef _DEBUG @@ -505,7 +585,7 @@ static MMAL_COMPONENT_T* create_camera_component(RASPIVID_STATE& state) { state.camera_component.reset(camera); - ROS_DEBUG("Camera component done\n"); + ROS_INFO("Camera component done\n"); return camera; @@ -751,8 +831,8 @@ int start_capture(RASPIVID_STATE& state) { MMAL_PORT_T* camera_video_port = state.camera_component->output[mmal::camera_port::video]; MMAL_PORT_T* splitter_output_raw = state.splitter_component->output[1]; - ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state.width, state.height/2, state.quality, state.framerate); - + // ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state.width, state.height/2, state.quality, state.framerate); // FIXME: + ROS_INFO("Starting video capture (width: %d, height: %d, quality: %d, framerate: %d)\n", state.new_width, state.new_height, state.quality, state.framerate); if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) { return 1; } @@ -775,39 +855,8 @@ int start_capture(RASPIVID_STATE& state) { return 0; } -int close_cam(RASPIVID_STATE& state) { - if (state.isInit) { - state.isInit = false; - MMAL_COMPONENT_T* camera = state.camera_component.get(); - MMAL_COMPONENT_T* splitter = state.splitter_component.get(); - - // Destroy splitter port connection - state.splitter_connection.reset(nullptr); - - - // Destroy splitter component - if (splitter) { - // Get rid of any port buffers first - state.splitter_pool.reset(nullptr); - // Delete callback structure - if (splitter->output[1]->userdata) { - delete splitter->output[1]->userdata; - } - state.splitter_component.reset(nullptr); - } - - // destroy camera component - if (camera) { - state.camera_component.reset(nullptr); - } - ROS_INFO("Video capture stopped\n"); - return 0; - } else - return 1; -} - void reconfigure_callback(raspicam_node::CameraConfig& config, uint32_t level, RASPIVID_STATE& state) { - ROS_DEBUG("figure Request: contrast %d, sharpness %d, brightness %d, " + ROS_INFO("figure Request: contrast %d, sharpness %d, brightness %d, " "saturation %d, ISO %d, exposureCompensation %d," " videoStabilisation %d, vFlip %d, hFlip %d," " zoom %.2f, exposure_mode %s, awb_mode %s, shutter_speed %d", @@ -828,24 +877,23 @@ void reconfigure_callback(raspicam_node::CameraConfig& config, uint32_t level, R PARAM_FLOAT_RECT_T roi; roi.x = roi.y = offset; roi.w = roi.h = size; - raspicamcontrol_set_ROI(state.camera_component.get(), roi); + IF_FALSE_SHUTDOWN(raspicamcontrol_set_ROI(state.camera_component.get(), roi), "Failed to set ROI") } - raspicamcontrol_set_exposure_mode(state.camera_component.get(), exposure_mode_from_string(config.exposure_mode.c_str())); - - raspicamcontrol_set_awb_mode(state.camera_component.get(), awb_mode_from_string(config.awb_mode.c_str())); + IF_FALSE_SHUTDOWN(raspicamcontrol_set_exposure_mode(state.camera_component.get(), exposure_mode_from_string(config.exposure_mode.c_str())), "Failed to set exposure mode") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_awb_mode(state.camera_component.get(), awb_mode_from_string(config.awb_mode.c_str())), "Failed to set AWB mode") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_contrast(state.camera_component.get() , config.contrast), "Failed to set contrast") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_sharpness(state.camera_component.get(), config.sharpness), "Failed to set sharpness") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_brightness(state.camera_component.get(), config.brightness), "Failed to set brightness") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_saturation(state.camera_component.get(), config.saturation), "Failed to set saturation") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_ISO(state.camera_component.get(), config.ISO) , "Failed to set ISO") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_exposure_compensation(state.camera_component.get(), config.exposure_compensation) , "Failed to set exposure compensation") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_video_stabilisation(state.camera_component.get(), config.video_stabilisation), "Failed to set video stabilisation") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_flips(state.camera_component.get(), config.hFlip, config.vFlip), "Failed to set flips") + IF_FALSE_SHUTDOWN(raspicamcontrol_set_shutter_speed(state.camera_component.get(), config.shutter_speed), "Failed to set shutter speed") - raspicamcontrol_set_contrast(state.camera_component.get(), config.contrast); - raspicamcontrol_set_sharpness(state.camera_component.get(), config.sharpness); - raspicamcontrol_set_brightness(state.camera_component.get(), config.brightness); - raspicamcontrol_set_saturation(state.camera_component.get(), config.saturation); - raspicamcontrol_set_ISO(state.camera_component.get(), config.ISO); - raspicamcontrol_set_exposure_compensation(state.camera_component.get(), config.exposure_compensation); - raspicamcontrol_set_video_stabilisation(state.camera_component.get(), config.video_stabilisation); - raspicamcontrol_set_flips(state.camera_component.get(), config.hFlip, config.vFlip); - raspicamcontrol_set_shutter_speed(state.camera_component.get(), config.shutter_speed); - ROS_DEBUG("Reconfigure done"); + ROS_INFO("Reconfigure done"); } int main(int argc, char** argv) { @@ -948,4 +996,4 @@ int main(int argc, char** argv) { ros::shutdown(); } -#endif // __arm__ +// #endif // __arm__ From e797c1625651f2ba8bfd02577c18846ee999fe38 Mon Sep 17 00:00:00 2001 From: lashhev Date: Fri, 14 Feb 2020 01:38:46 +0300 Subject: [PATCH 16/18] refactoring --- src/stereo_node.cpp | 125 +++++++++++++++++++++----------------------- 1 file changed, 60 insertions(+), 65 deletions(-) diff --git a/src/stereo_node.cpp b/src/stereo_node.cpp index 675a437..3c7cc1f 100644 --- a/src/stereo_node.cpp +++ b/src/stereo_node.cpp @@ -170,16 +170,16 @@ sensor_msgs::CameraInfo right_info; std::string camera_frame_id; int skip_frames = 0; -bool resize(sensor_msgs::Image& source_msg, +bool resize(cv::Mat& source_image, sensor_msgs::Image& resized_msg, cv::Size size, std::string encoding) { - // ROS_WARN("Starting resizing ..."); - cv_bridge::CvImagePtr bridge = cv_bridge::toCvCopy(source_msg, encoding); - cv::resize(bridge->image, bridge->image, size, 0, 0, cv::INTER_LINEAR); - // cv::imshow("resized", image); - // cv::waitKey(1); - resized_msg = *bridge->toImageMsg().get(); - // ROS_WARN("Resizing finished..."); + cv::Mat resized_image; + cv::resize(source_image, resized_image, size, 0, 0, cv::INTER_LINEAR); + cv_bridge::CvImage bridge; + bridge.image = resized_image; + bridge.header = resized_msg.header; + bridge.encoding = resized_msg.encoding; + resized_msg = *bridge.toImageMsg().get(); return true; } @@ -224,17 +224,7 @@ int close_cam(RASPIVID_STATE& state) { static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { nh.param("imagefx_mode", state.imagefx_mode, "denoise"); nh.param("encoding", state.encoding, "bgr8"); - if(state.encoding == "mono8") - state.encoding = sensor_msgs::image_encodings::MONO8; - else if(state.encoding == "bgr8") - { - state.encoding = sensor_msgs::image_encodings::BGR8; - } - else - { - ROS_WARN("Unknown encoding. Set to default"); - state.encoding = sensor_msgs::image_encodings::BGR8; - } + nh.param("width", state.new_width, 1280); nh.param("height", state.new_height, 720); @@ -264,7 +254,19 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { nh.param("vFlip", temp, false); state.camera_parameters.vflip = temp; // Hack for bool param => int variable nh.param("shutter_speed", state.camera_parameters.shutter_speed, 0); - + + if(state.encoding == "mono8") + state.encoding = sensor_msgs::image_encodings::MONO8; + // state.camera_parameters.colourEffects.enable = 1; + else if(state.encoding == "bgr8") + { + state.encoding = sensor_msgs::image_encodings::BGR8; + } + else + { + ROS_WARN("Unknown encoding. Set to default"); + state.encoding = sensor_msgs::image_encodings::BGR8; + } state.isInit = false; } @@ -276,20 +278,18 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { * @param port Pointer to port from which callback originated * @param buffer mmal buffer header pointer */ -void update_image(sensor_msgs::Image& msg, MMAL_BUFFER_HEADER_T* buffer, PORT_USERDATA* pData, ros::Time ts) +void update_image(sensor_msgs::Image& msg, PORT_USERDATA* pData, ros::Time ts) { msg.header.frame_id = camera_frame_id; msg.header.stamp = ts; //(double(buffer->dts)/1e6); - msg.encoding = "bgr8"; -// #ifdef _DEBUG -// DBG_MSG("//DEBUG// - buffer->dts:") -// DBG_MSG( buffer->dts) -// DBG_MSG(MMAL_TIME_UNKNOWN) -// #endif // _DEBUG + msg.encoding = pData->pstate.encoding; msg.is_bigendian = false; - msg.height = pData->pstate.height/2; - msg.width = pData->pstate.width; - msg.step = (pData->pstate.width * 3); + msg.height = pData->pstate.new_height; + msg.width = pData->pstate.new_width; + uint8_t channels = 1; + if(pData->pstate.encoding == sensor_msgs::image_encodings::BGR8) + channels = 3; + msg.step = (pData->pstate.width * channels); } static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* buffer) { // We pass our file handle and other stuff in via the userdata field. @@ -331,44 +331,38 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu } else { pData->frames_skipped = 0; - update_image(left_image.msg, buffer, pData, ts); - update_image(right_image.msg, buffer, pData, ts); + update_image(left_image.msg, pData, ts); + update_image(right_image.msg, pData, ts); auto start = pData->buffer[pData->frame & 1].get(); auto end = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); auto start1 = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); auto end1 = &(pData->buffer[pData->frame & 1].get()[pData->id]); - left_image.msg.data.resize(pData->id/2); - #ifdef _DEBUG - DBG_MSG("//DEBUG// - Fill in left image!!!") - #endif // _DEBUG - std::copy(start1, end1, left_image.msg.data.begin()); - #ifdef _DEBUG - DBG_MSG("//DEBUG// - Success") - #endif // _DEBUG - right_image.msg.data.resize(pData->id/2); - #ifdef _DEBUG - DBG_MSG("//DEBUG// - Fill in right image!!!") - #endif // _DEBUG - std::copy(start, end, right_image.msg.data.begin()); - #ifdef _DEBUG - DBG_MSG("//DEBUG// - Success!!!") - DBG_MSG("//DEBUG// - Publishing messages!!!") - print_image(left_image.msg); - print_image(right_image.msg); - #endif // _DEBUG - // ROS_WARN("Publishing images starting..."); - sensor_msgs::Image image_msg; - resize(left_image.msg, image_msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); - // ROS_WARN("Publishing left image starting..."); - left_image.msg = image_msg; - left_image.pub->publish(left_image.msg); - resize(right_image.msg, image_msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); - right_image.msg = image_msg; - right_image.pub->publish(right_image.msg); - - #ifdef _DEBUG - DBG_MSG("//DEBUG// - Success!!!") - #endif // _DEBUG + int mat_encoding = CV_8UC3; + // if(pData->pstate.encoding == sensor_msgs::image_encodings::BGR8) + // mat_encoding = CV_8UC3; + cv::Mat left_image_cv(cv::Size(pData->pstate.width, pData->pstate.height/2), mat_encoding ); + cv::Mat right_image_cv(cv::Size(pData->pstate.width, pData->pstate.height/2), mat_encoding); + // left_image.msg.data.resize(pData->id/2); + // right_image.msg.data.resize(pData->id/2); + // std::copy(start1, end1, left_image.msg.data.begin()); + std::copy(start, end, left_image_cv.data); + std::copy(start1, end1, right_image_cv.data); + + if(pData->pstate.encoding == sensor_msgs::image_encodings::MONO8) + { + cv::cvtColor(left_image_cv, left_image_cv, cv::COLOR_BGR2GRAY); + cv::cvtColor(right_image_cv, right_image_cv, cv::COLOR_BGR2GRAY); + } + // cv::imwrite("/home/pi/Documents/left/image.png", left_image_cv); + // cv::imwrite("/home/pi/Documents/right/image.png", right_image_cv); + // std::copy(start, end, right_image.msg.data.begin()); + resize(left_image_cv, left_image.msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); + resize(right_image_cv, right_image.msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); + // ROS_WARN("Publishing left image starting..."); + left_image.pub->publish(left_image.msg); + right_image.pub->publish(right_image.msg); + + left_info.header = left_image.msg.header; right_info.header = left_image.msg.header; left_camera_info_pub.publish(left_info); @@ -674,9 +668,10 @@ static MMAL_STATUS_T create_splitter_component(RASPIVID_STATE& state) { // Use BGR24 (bgr8 in ROS) format = splitter_output_raw->format; + format->encoding = MMAL_ENCODING_BGR24; format->encoding_variant = 0; /* Irrelevant when not in opaque mode */ - + status = mmal_port_format_commit(splitter_output_raw); if (status != MMAL_SUCCESS) { From 2907b54a15467d52b832ae09836d1449bf08e7dd Mon Sep 17 00:00:00 2001 From: lashhev Date: Fri, 15 May 2020 06:21:32 +0300 Subject: [PATCH 17/18] optimize --- CMakeLists.txt | 2 +- launch/stereo_1280x720.launch | 8 +++---- src/stereo_node.cpp | 41 +++++++++++++++++++---------------- 3 files changed, 27 insertions(+), 24 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e2c02da..4b58496 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -127,7 +127,7 @@ include_directories(include ${OpenCV_INCLUDE_DIRS} ) -add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral) +add_compile_options(-Wall -Wuseless-cast -Wformat-nonliteral -Wparentheses) ## Declare a cpp library add_library(raspicli STATIC diff --git a/launch/stereo_1280x720.launch b/launch/stereo_1280x720.launch index 61cbd22..5230c6e 100644 --- a/launch/stereo_1280x720.launch +++ b/launch/stereo_1280x720.launch @@ -1,10 +1,10 @@ - + - + @@ -20,8 +20,8 @@ - - + + diff --git a/src/stereo_node.cpp b/src/stereo_node.cpp index 3c7cc1f..8cae010 100644 --- a/src/stereo_node.cpp +++ b/src/stereo_node.cpp @@ -94,8 +94,8 @@ extern "C" { #include #include #include - -static constexpr int IMG_BUFFER_SIZE = 10 * 1024 * 1024; // 10 MB +// 16 193 408*3 = 48 580 224 +static constexpr int IMG_BUFFER_SIZE = 50 * 1024 * 1024; // 10 MB // Video format information static constexpr int VIDEO_FRAME_RATE_DEN = 1; @@ -114,8 +114,6 @@ struct RASPIVID_STATE { bool isInit; int width; /// Requested width of image int height; /// requested height of image - int new_width; - int new_height; int framerate; /// Requested frame rate (fps) int quality; int camera_id = 0; @@ -170,13 +168,13 @@ sensor_msgs::CameraInfo right_info; std::string camera_frame_id; int skip_frames = 0; -bool resize(cv::Mat& source_image, - sensor_msgs::Image& resized_msg, cv::Size size, std::string encoding) +bool encode(cv::Mat& source_image, + sensor_msgs::Image& resized_msg, std::string encoding) { - cv::Mat resized_image; - cv::resize(source_image, resized_image, size, 0, 0, cv::INTER_LINEAR); + // cv::Mat resized_image; + // cv::resize(source_image, resized_image, size, 0, 0, cv::INTER_LINEAR); cv_bridge::CvImage bridge; - bridge.image = resized_image; + bridge.image = source_image; bridge.header = resized_msg.header; bridge.encoding = resized_msg.encoding; resized_msg = *bridge.toImageMsg().get(); @@ -226,10 +224,9 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { nh.param("encoding", state.encoding, "bgr8"); - nh.param("width", state.new_width, 1280); - nh.param("height", state.new_height, 720); - state.width = 1280; - state.height = 720*2; + nh.param("width", state.width, 640); + nh.param("height", state.height, 480); + state.height*=2; nh.param("quality", state.quality, 100); if (state.quality < 0 && state.quality > 100) { ROS_WARN("quality: %d is outside valid range 0-100, defaulting to 80", state.quality); @@ -284,8 +281,8 @@ void update_image(sensor_msgs::Image& msg, PORT_USERDATA* pData, ros::Time ts) msg.header.stamp = ts; //(double(buffer->dts)/1e6); msg.encoding = pData->pstate.encoding; msg.is_bigendian = false; - msg.height = pData->pstate.new_height; - msg.width = pData->pstate.new_width; + msg.height = pData->pstate.height; + msg.width = pData->pstate.width; uint8_t channels = 1; if(pData->pstate.encoding == sensor_msgs::image_encodings::BGR8) channels = 3; @@ -337,6 +334,11 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu auto end = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); auto start1 = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); auto end1 = &(pData->buffer[pData->frame & 1].get()[pData->id]); + if(start == NULL || start1 == NULL || end == NULL || end1 == NULL) + { + ROS_WARN("Failed to capture image. Empty buffer!"); + return; + } int mat_encoding = CV_8UC3; // if(pData->pstate.encoding == sensor_msgs::image_encodings::BGR8) // mat_encoding = CV_8UC3; @@ -356,8 +358,8 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu // cv::imwrite("/home/pi/Documents/left/image.png", left_image_cv); // cv::imwrite("/home/pi/Documents/right/image.png", right_image_cv); // std::copy(start, end, right_image.msg.data.begin()); - resize(left_image_cv, left_image.msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); - resize(right_image_cv, right_image.msg, cv::Size(pData->pstate.new_width, pData->pstate.new_height), pData->pstate.encoding); + encode(left_image_cv, left_image.msg, pData->pstate.encoding); + encode(right_image_cv, right_image.msg, pData->pstate.encoding); // ROS_WARN("Publishing left image starting..."); left_image.pub->publish(left_image.msg); right_image.pub->publish(right_image.msg); @@ -827,7 +829,7 @@ int start_capture(RASPIVID_STATE& state) { MMAL_PORT_T* camera_video_port = state.camera_component->output[mmal::camera_port::video]; MMAL_PORT_T* splitter_output_raw = state.splitter_component->output[1]; // ROS_INFO("Starting video capture (%d, %d, %d, %d)\n", state.width, state.height/2, state.quality, state.framerate); // FIXME: - ROS_INFO("Starting video capture (width: %d, height: %d, quality: %d, framerate: %d)\n", state.new_width, state.new_height, state.quality, state.framerate); + ROS_INFO("Starting video capture (width: %d, height: %d, quality: %d, framerate: %d)\n", state.width, state.height/2, state.quality, state.framerate); if (mmal_port_parameter_set_boolean(camera_video_port, MMAL_PARAMETER_CAPTURE, 1) != MMAL_SUCCESS) { return 1; } @@ -986,7 +988,8 @@ int main(int argc, char** argv) { DBG_MSG("//DEBUG// - start_capture!!!") #endif // _DEBUG start_capture(state_srv); - ros::spin(); + ros::MultiThreadedSpinner spinner(4); // Use 4 threads + spinner.spin(); // spin() will not return until the node has been shutdown close_cam(state_srv); ros::shutdown(); } From 77dd730c491db64e80a7c02db475f3e8df6c3891 Mon Sep 17 00:00:00 2001 From: lashhev Date: Mon, 18 May 2020 15:14:43 +0300 Subject: [PATCH 18/18] fix camera info load --- camera_info/left/left.yaml | 26 ++++++++++++++++++++++++++ camera_info/left/ost.yaml | 26 ++++++++++++++++++++++++++ camera_info/right/ost.yaml | 26 ++++++++++++++++++++++++++ camera_info/right/right.yaml | 26 ++++++++++++++++++++++++++ launch/stereo_1280x720.launch | 11 ++++++----- src/stereo_node.cpp | 27 ++++++++++++++------------- 6 files changed, 124 insertions(+), 18 deletions(-) create mode 100644 camera_info/left/left.yaml create mode 100644 camera_info/left/ost.yaml create mode 100644 camera_info/right/ost.yaml create mode 100644 camera_info/right/right.yaml diff --git a/camera_info/left/left.yaml b/camera_info/left/left.yaml new file mode 100644 index 0000000..e2ed20d --- /dev/null +++ b/camera_info/left/left.yaml @@ -0,0 +1,26 @@ +image_width: 640 +image_height: 480 +camera_name: left_camera_optical_frame +camera_matrix: + rows: 3 + cols: 3 + data: [ 383.37648, 0. , 318.72515, + 0. , 386.12691, 251.37268, + 0. , 0. , 1. ] +camera_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.294786, 0.071835, -0.004425, 0.000128, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 0.99380801, -0.0319161 , 0.10642835, + 0.03129902, 0.9994822 , 0.00746371, + -0.10661146, -0.00408639, 0.99429236] +projection_matrix: + rows: 3 + cols: 4 + data: [ 357.83988, 0. , 263.50254, 0. , + 0. , 357.83988, 249.2244 , 0. , + 0. , 0. , 1. , 0. ] \ No newline at end of file diff --git a/camera_info/left/ost.yaml b/camera_info/left/ost.yaml new file mode 100644 index 0000000..6db1244 --- /dev/null +++ b/camera_info/left/ost.yaml @@ -0,0 +1,26 @@ +image_width: 640 +image_height: 480 +camera_name: left_camera_optical_frame +camera_matrix: + rows: 3 + cols: 3 + data: [ 377.34204, 0. , 317.17444, + 0. , 377.98404, 242.82206, + 0. , 0. , 1. ] +camera_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.317285, 0.096353, -0.000721, 0.000562, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [ 284.43878, 0. , 317.7113 , 0. , + 0. , 322.88959, 243.25744, 0. , + 0. , 0. , 1. , 0. ] diff --git a/camera_info/right/ost.yaml b/camera_info/right/ost.yaml new file mode 100644 index 0000000..481dc06 --- /dev/null +++ b/camera_info/right/ost.yaml @@ -0,0 +1,26 @@ +image_width: 640 +image_height: 480 +camera_name: right_camera_optical_frame +camera_matrix: + rows: 3 + cols: 3 + data: [ 370.55914, 0. , 329.09807, + 0. , 371.35797, 257.59864, + 0. , 0. , 1. ] +camera_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.324288, 0.107501, -0.001059, 0.000672, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [ 289.42285, 0. , 328.57791, 0. , + 0. , 314.12253, 263.15155, 0. , + 0. , 0. , 1. , 0. ] diff --git a/camera_info/right/right.yaml b/camera_info/right/right.yaml new file mode 100644 index 0000000..8d35279 --- /dev/null +++ b/camera_info/right/right.yaml @@ -0,0 +1,26 @@ +image_width: 640 +image_height: 480 +camera_name: right_camera_optical_frame +camera_matrix: + rows: 3 + cols: 3 + data: [ 378.37387, 0. , 329.03434, + 0. , 380.28319, 260.9376 , + 0. , 0. , 1. ] +camera_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.305682, 0.078253, -0.001969, 0.000841, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [ 0.99584801, -0.02643552, 0.08710852, + 0.02693943, 0.99962642, -0.00461425, + -0.086954 , 0.00694175, 0.99618814] +projection_matrix: + rows: 3 + cols: 4 + data: [ 357.83988, 0. , 263.50254, -23.39691, + 0. , 357.83988, 249.2244 , 0. , + 0. , 0. , 1. , 0. ] \ No newline at end of file diff --git a/launch/stereo_1280x720.launch b/launch/stereo_1280x720.launch index 5230c6e..faeeba3 100644 --- a/launch/stereo_1280x720.launch +++ b/launch/stereo_1280x720.launch @@ -1,27 +1,28 @@ - - + + + - - + + - + diff --git a/src/stereo_node.cpp b/src/stereo_node.cpp index 8cae010..ba50bfb 100644 --- a/src/stereo_node.cpp +++ b/src/stereo_node.cpp @@ -166,6 +166,7 @@ ros::Publisher right_camera_info_pub; sensor_msgs::CameraInfo left_info; sensor_msgs::CameraInfo right_info; std::string camera_frame_id; +std::string left_camera_name, right_camera_name; int skip_frames = 0; bool encode(cv::Mat& source_image, @@ -238,7 +239,7 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { state.framerate = 30; } - nh.param("camera_frame_id", camera_frame_id, "stereo"); + // nh.param("camera_frame_id", camera_frame_id, "stereo"); nh.param("camera_id", state.camera_id, 0); @@ -275,9 +276,9 @@ static void configure_parameters(RASPIVID_STATE& state, ros::NodeHandle& nh) { * @param port Pointer to port from which callback originated * @param buffer mmal buffer header pointer */ -void update_image(sensor_msgs::Image& msg, PORT_USERDATA* pData, ros::Time ts) +void update_image(sensor_msgs::Image& msg, PORT_USERDATA* pData, ros::Time ts, const std::string& frame_name) { - msg.header.frame_id = camera_frame_id; + msg.header.frame_id = frame_name; msg.header.stamp = ts; //(double(buffer->dts)/1e6); msg.encoding = pData->pstate.encoding; msg.is_bigendian = false; @@ -328,8 +329,8 @@ static void splitter_buffer_callback(MMAL_PORT_T* port, MMAL_BUFFER_HEADER_T* bu } else { pData->frames_skipped = 0; - update_image(left_image.msg, pData, ts); - update_image(right_image.msg, pData, ts); + update_image(left_image.msg, pData, ts, left_camera_name); + update_image(right_image.msg, pData, ts, right_camera_name); auto start = pData->buffer[pData->frame & 1].get(); auto end = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); auto start1 = &(pData->buffer[pData->frame & 1].get()[pData->id/2]); @@ -911,7 +912,7 @@ int main(int argc, char** argv) { nh_params.param("skip_frames", skip_frames, 0); std::string left_camera_info_url, right_camera_info_url; - std::string left_camera_name, right_camera_name; + nh_params.param("left_camera_info_url", left_camera_info_url, std::string("package://raspicam_node/camera_info/left_camera.yaml")); nh_params.param("right_camera_info_url", right_camera_info_url, std::string("package://raspicam_node/camera_info/right_camera.yaml")); @@ -919,7 +920,7 @@ int main(int argc, char** argv) { nh_params.param("right_camera_name", right_camera_name, std::string("right_camera")); camera_info_manager::CameraInfoManager left_info_man(nh_params, left_camera_name, left_camera_info_url); - // camera_info_manager::CameraInfoManager right_info_man(nh_params, right_camera_name, right_camera_info_url); + camera_info_manager::CameraInfoManager right_info_man(nh_params, right_camera_name, right_camera_info_url); RASPIVID_STATE state_srv; #ifdef _DEBUG @@ -944,10 +945,10 @@ int main(int argc, char** argv) { #ifdef _DEBUG DBG_MSG("//DEBUG// - Load right camera calibration!!!") #endif // _DEBUG - if (!left_info_man.loadCameraInfo(right_camera_info_url)) { + if (!right_info_man.loadCameraInfo(right_camera_info_url)) { ROS_INFO("Calibration file missing. Right camera not calibrated"); } else { - right_info = left_info_man.getCameraInfo(); + right_info = right_info_man.getCameraInfo(); ROS_INFO("Camera successfully calibrated from config file"); } @@ -963,10 +964,10 @@ int main(int argc, char** argv) { #ifdef _DEBUG DBG_MSG("//DEBUG// - Advertizing!!!") #endif // _DEBUG - auto left_image_pub = nh_topics.advertise("left_image", 2); + auto left_image_pub = nh_topics.advertise("left/image_raw", 2); left_image.pub.reset(new DiagnosedPublisher( left_image_pub, state_srv.updater, FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10), TimeStampStatusParam(0, 0.2))); // if (state_srv.enable_imv_pub) { - auto right_image_pub = nh_topics.advertise("right_image", 2); + auto right_image_pub = nh_topics.advertise("right/image_raw", 2); right_image.pub.reset(new DiagnosedPublisher( right_image_pub, state_srv.updater, FrequencyStatusParam(&min_freq, &max_freq, 0.1, 10), TimeStampStatusParam(0, 0.2))); if(left_image.pub == nullptr || right_image.pub == nullptr) @@ -974,8 +975,8 @@ int main(int argc, char** argv) { ROS_INFO("Publisher is corrupted"); return 0; } - left_camera_info_pub = nh_topics.advertise("left_camera_info", 2); - right_camera_info_pub = nh_topics.advertise("right_camera_info", 2); + left_camera_info_pub = nh_topics.advertise("left/camera_info", 2); + right_camera_info_pub = nh_topics.advertise("right/camera_info", 2); dynamic_reconfigure::Server server; dynamic_reconfigure::Server::CallbackType f;