Skip to content

Commit

Permalink
Merge pull request #154 from mdhorn/add-catches
Browse files Browse the repository at this point in the history 
Add catches
  • Loading branch information
@mdhorn
mdhorn authored Nov 18, 2016
2 parents 3884210 + 1a06323 commit ab26677
Show file tree
Hide file tree
Showing 4 changed files with 102 additions and 14 deletions.
4 changes: 2 additions & 2 deletions realsense_camera/include/realsense_camera/base_nodelet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,8 +100,8 @@ namespace realsense_camera
ros::ServiceServer force_power_service_;
ros::ServiceServer is_powered_service_;
tf2_ros::StaticTransformBroadcaster static_tf_broadcaster_;
rs_error *rs_error_ = 0;
rs_context *rs_context_;
rs_error *rs_error_ = NULL;
rs_context *rs_context_ = NULL;
rs_device *rs_device_;
std::string nodelet_name_;
std::string serial_no_;
Expand Down
77 changes: 71 additions & 6 deletions realsense_camera/src/base_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,17 +45,24 @@ namespace realsense_camera

stopCamera();

rs_delete_context(rs_context_, &rs_error_);
checkError();
if (rs_context_)
{
rs_delete_context(rs_context_, &rs_error_);
rs_context_ = NULL;
checkError();
}

ROS_INFO_STREAM(nodelet_name_ << " - Stopping...");
ros::shutdown();
if (! ros::isShuttingDown())
{
ros::shutdown();
}
}

/*
* Initialize the nodelet.
*/
void BaseNodelet::onInit()
void BaseNodelet::onInit() try
{
getParameters();

Expand Down Expand Up @@ -92,6 +99,23 @@ namespace realsense_camera
// Start dynamic reconfigure callback
startDynamicReconfCallback();
}
catch(const rs::error & e)
{
ROS_ERROR_STREAM(nodelet_name_ << " - " << "RealSense error calling "
<< e.get_failed_function() << "(" << e.get_failed_args() << "):\n "
<< e.what());
ros::shutdown();
}
catch(const std::exception & e)
{
ROS_ERROR_STREAM(nodelet_name_ << " - " << e.what());
ros::shutdown();
}
catch(...)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Caught unknown expection...shutting down!");
ros::shutdown();
}

/*
* Get the nodelet parameters.
Expand Down Expand Up @@ -131,6 +155,10 @@ namespace realsense_camera
bool BaseNodelet::connectToCamera()
{
rs_context_ = rs_create_context(RS_API_VERSION, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - No cameras detected!");
}
checkError();

int num_of_cameras = rs_get_device_count(rs_context_, &rs_error_);
Expand All @@ -141,6 +169,7 @@ namespace realsense_camera
{
ROS_ERROR_STREAM(nodelet_name_ << " - No cameras detected!");
rs_delete_context(rs_context_, &rs_error_);
rs_context_ = NULL;
checkError();
return false;
}
Expand All @@ -153,6 +182,7 @@ namespace realsense_camera
{
ROS_ERROR_STREAM(nodelet_name_ << " - No '" << camera_type_ << "' cameras detected!");
rs_delete_context(rs_context_, &rs_error_);
rs_context_ = NULL;
checkError();
return false;
}
Expand All @@ -163,6 +193,7 @@ namespace realsense_camera
ROS_ERROR_STREAM(nodelet_name_ <<
" - Multiple cameras of same type detected but no input serial_no or usb_port_id specified");
rs_delete_context(rs_context_, &rs_error_);
rs_context_ = NULL;
checkError();
return false;
}
Expand Down Expand Up @@ -197,6 +228,7 @@ namespace realsense_camera
ROS_ERROR_STREAM(nodelet_name_ << error_msg);

rs_delete_context(rs_context_, &rs_error_);
rs_context_ = NULL;
checkError();
return false;
}
Expand Down Expand Up @@ -569,6 +601,10 @@ namespace realsense_camera
{
rs_intrinsics intrinsic;
rs_get_stream_intrinsics(rs_device_, stream_index, &intrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera firmware version and/or calibration data!");
}
checkError();

sensor_msgs::CameraInfo * camera_info = new sensor_msgs::CameraInfo();
Expand Down Expand Up @@ -604,6 +640,10 @@ namespace realsense_camera
// set depth to color translation values in Projection matrix (P)
rs_extrinsics z_extrinsic;
rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
}
checkError();
camera_info->P.at(3) = z_extrinsic.translation[0]; // Tx
camera_info->P.at(7) = z_extrinsic.translation[1]; // Ty
Expand Down Expand Up @@ -703,7 +743,7 @@ namespace realsense_camera
/*
* Prepare and publish topics.
*/
void BaseNodelet::prepareTopics()
void BaseNodelet::prepareTopics() try
{
while (ros::ok())
{
Expand Down Expand Up @@ -754,6 +794,23 @@ namespace realsense_camera
}
}
}
catch(const rs::error & e)
{
ROS_ERROR_STREAM(nodelet_name_ << " - " << "RealSense error calling "
<< e.get_failed_function() << "(" << e.get_failed_args() << "):\n "
<< e.what());
ros::shutdown();
}
catch(const std::exception & e)
{
ROS_ERROR_STREAM(nodelet_name_ << " - " << e.what());
ros::shutdown();
}
catch(...)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Caught unknown expection...shutting down!");
ros::shutdown();
}

/*
* Call publicTopic() for all streams.
Expand Down Expand Up @@ -969,6 +1026,10 @@ namespace realsense_camera

// Get offset between base frame and depth frame
rs_get_device_extrinsics(rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
}
checkError();

// Transform base frame to depth frame
Expand Down Expand Up @@ -1000,6 +1061,10 @@ namespace realsense_camera

// Get offset between base frame and infrared frame
rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
}
checkError();

// Transform base frame to infrared frame
Expand Down Expand Up @@ -1040,8 +1105,8 @@ namespace realsense_camera
ROS_ERROR_STREAM(nodelet_name_ << " - Error calling " << rs_get_failed_function(rs_error_) << " ( "
<< rs_get_failed_args(rs_error_) << " ): \n" << rs_get_error_message(rs_error_) << " \n");
rs_free_error(rs_error_);
rs_error_ = NULL;
ros::shutdown();
exit (EXIT_FAILURE);
}
}
} // end namespace
4 changes: 4 additions & 0 deletions realsense_camera/src/r200_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -489,6 +489,10 @@ namespace realsense_camera

// Get offset between base frame and infrared2 frame
rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED2, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
}
checkError();

// Transform base frame to infrared2 frame
Expand Down
31 changes: 25 additions & 6 deletions realsense_camera/src/zr300_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,10 @@ namespace realsense_camera

rs_motion_intrinsics imu_intrinsics;
rs_get_motion_intrinsics(rs_device_, &imu_intrinsics, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera firmware version!");
}
checkError();

int index = 0;
Expand Down Expand Up @@ -700,6 +704,10 @@ namespace realsense_camera

// Get offset between base frame and infrared2 frame
rs_get_device_extrinsics(rs_device_, RS_STREAM_INFRARED2, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
}
checkError();

// Transform base frame to infrared2 frame
Expand Down Expand Up @@ -731,6 +739,10 @@ namespace realsense_camera

// Get offset between base frame and fisheye frame
rs_get_device_extrinsics(rs_device_, RS_STREAM_FISHEYE, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
if (rs_error_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
}
checkError();

// Transform base frame to fisheye frame
Expand Down Expand Up @@ -762,14 +774,21 @@ namespace realsense_camera


// Get offset between base frame and imu frame
/* Temporarily commenting out the code. Instead, hardcoding the values until fully supported by librealsense API.

rs_get_motion_extrinsics_from(rs_device_, RS_STREAM_COLOR, &z_extrinsic, &rs_error_);
checkError();
*/
z_extrinsic.translation[0] = -0.07;
z_extrinsic.translation[1] = 0.0;
z_extrinsic.translation[2] = 0.0;
if (rs_error_)
{
/* Temporarily hardcoding the values until fully supported by librealsense API. */
// ROS_ERROR_STREAM(nodelet_name_ << " - Verify camera is calibrated!");
ROS_WARN_STREAM(nodelet_name_ << " - Using Hardcoded extrinsic for IMU.");
rs_free_error(rs_error_);
rs_error_ = NULL;

z_extrinsic.translation[0] = -0.07;
z_extrinsic.translation[1] = 0.0;
z_extrinsic.translation[2] = 0.0;
}
//checkError();

// Transform base frame to imu frame
b2imu_msg.header.stamp = static_transform_ts_;
Expand Down

0 comments on commit ab26677

Please sign in to comment.