diff --git a/orbbec_camera/include/orbbec_camera/ob_camera_node.h b/orbbec_camera/include/orbbec_camera/ob_camera_node.h index 728f1165..0f81da96 100644 --- a/orbbec_camera/include/orbbec_camera/ob_camera_node.h +++ b/orbbec_camera/include/orbbec_camera/ob_camera_node.h @@ -425,6 +425,7 @@ class OBCameraNode { std::unique_ptr imuPipeline_ = nullptr; std::atomic_bool pipeline_started_{false}; std::string camera_name_ = "camera"; + std::string namespace_ = ""; std::string accel_gyro_frame_id_ = "camera_accel_gyro_optical_frame"; const std::string imu_frame_id_ = "camera_gyro_frame"; std::shared_ptr pipeline_config_ = nullptr; diff --git a/orbbec_camera/src/ob_camera_node.cpp b/orbbec_camera/src/ob_camera_node.cpp index 6f405fd1..31eef53f 100644 --- a/orbbec_camera/src/ob_camera_node.cpp +++ b/orbbec_camera/src/ob_camera_node.cpp @@ -1520,7 +1520,8 @@ void OBCameraNode::setupDefaultImageFormat() { void OBCameraNode::getParameters() { setAndGetNodeParameter(camera_name_, "camera_name", "camera"); - camera_link_frame_id_ = camera_name_ + "_link"; + setAndGetNodeParameter(namespace_, "namespace", ""); + camera_link_frame_id_ = namespace_ + "/" + camera_name_ + "_link"; for (auto stream_index : IMAGE_STREAMS) { std::string param_name = stream_name_[stream_index] + "_width"; setAndGetNodeParameter(width_[stream_index], param_name, 0); @@ -1540,11 +1541,11 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(mirror_stream_[stream_index], param_name, false); param_name = stream_name_[stream_index] + "_rotation"; setAndGetNodeParameter(rotation_stream_[stream_index], param_name, -1); - param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id"; + param_name = namespace_ + "/" + camera_name_ + "_" + stream_name_[stream_index] + "_frame_id"; std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame"; setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id); std::string default_optical_frame_id = - camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame"; + namespace_ + "/" + camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame"; param_name = stream_name_[stream_index] + "_optical_frame_id"; setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id); param_name = stream_name_[stream_index] + "_format"; @@ -1561,7 +1562,7 @@ void OBCameraNode::getParameters() { depth_aligned_frame_id_[stream_index] = optical_frame_id_[COLOR]; } - accel_gyro_frame_id_ = camera_name_ + "_accel_gyro_optical_frame"; + accel_gyro_frame_id_ = namespace_ + "/" + camera_name_ + "_accel_gyro_optical_frame"; setAndGetNodeParameter(enable_sync_output_accel_gyro_, "enable_sync_output_accel_gyro", false); @@ -1577,15 +1578,15 @@ void OBCameraNode::getParameters() { setAndGetNodeParameter(imu_rate_[stream_index], param_name, ""); param_name = stream_name_[stream_index] + "_range"; setAndGetNodeParameter(imu_range_[stream_index], param_name, ""); - param_name = camera_name_ + "_" + stream_name_[stream_index] + "_frame_id"; - std::string default_frame_id = camera_name_ + "_" + stream_name_[stream_index] + "_frame"; + param_name = namespace_ + "/" + camera_name_ + "_" + stream_name_[stream_index] + "_frame_id"; + std::string default_frame_id = namespace_ + "/" + camera_name_ + "_" + stream_name_[stream_index] + "_frame"; setAndGetNodeParameter(frame_id_[stream_index], param_name, default_frame_id); std::string default_optical_frame_id = - camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame"; + namespace_ + "/" + camera_name_ + "_" + stream_name_[stream_index] + "_optical_frame"; param_name = stream_name_[stream_index] + "_optical_frame_id"; setAndGetNodeParameter(optical_frame_id_[stream_index], param_name, default_optical_frame_id); depth_aligned_frame_id_[stream_index] = - camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame"; + namespace_ + "/" + camera_name_ + "_" + stream_name_[COLOR] + "_optical_frame"; } setAndGetNodeParameter(publish_tf_, "publish_tf", true); setAndGetNodeParameter(tf_publish_rate_, "tf_publish_rate", 0.0); diff --git a/orbbec_camera/src/ros_service.cpp b/orbbec_camera/src/ros_service.cpp index 60c82452..38b3d57d 100644 --- a/orbbec_camera/src/ros_service.cpp +++ b/orbbec_camera/src/ros_service.cpp @@ -29,7 +29,7 @@ void OBCameraNode::setupCameraCtrlServices() { continue; } auto stream_name = stream_name_[stream_index]; - std::string service_name = "get_" + stream_name + "_exposure"; + std::string service_name = camera_name_ + "/" + "get_" + stream_name + "_exposure"; get_exposure_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, @@ -37,14 +37,14 @@ void OBCameraNode::setupCameraCtrlServices() { getExposureCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_exposure"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_exposure"; set_exposure_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, std::shared_ptr response) { setExposureCallback(request, response, stream_index); }); - service_name = "get_" + stream_name + "_gain"; + service_name = camera_name_ + "/" + "get_" + stream_name + "_gain"; get_gain_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, @@ -52,14 +52,14 @@ void OBCameraNode::setupCameraCtrlServices() { getGainCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_gain"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_gain"; set_gain_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, std::shared_ptr response) { setGainCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_auto_exposure"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_auto_exposure"; set_auto_exposure_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, @@ -67,7 +67,7 @@ void OBCameraNode::setupCameraCtrlServices() { setAutoExposureCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_ae_roi"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_ae_roi"; set_ae_roi_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, @@ -75,7 +75,7 @@ void OBCameraNode::setupCameraCtrlServices() { setAeRoiCallback(request, response, stream_index); }); - service_name = "toggle_" + stream_name; + service_name = camera_name_ + "/" + "toggle_" + stream_name; toggle_sensor_srv_[stream_index] = node_->create_service( service_name, @@ -83,21 +83,21 @@ void OBCameraNode::setupCameraCtrlServices() { std::shared_ptr response) { toggleSensorCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_mirror"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_mirror"; set_mirror_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, std::shared_ptr response) { setMirrorCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_flip"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_flip"; set_flip_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, std::shared_ptr response) { setFlipCallback(request, response, stream_index); }); - service_name = "set_" + stream_name + "_rotation"; + service_name = camera_name_ + "/" + "set_" + stream_name + "_rotation"; set_rotation_srv_[stream_index] = node_->create_service( service_name, [this, stream_index = stream_index](const std::shared_ptr request, @@ -106,30 +106,30 @@ void OBCameraNode::setupCameraCtrlServices() { }); } set_fan_work_mode_srv_ = node_->create_service( - "set_fan_work_mode", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_fan_work_mode", [this](const std::shared_ptr request, std::shared_ptr response) { setFanWorkModeCallback(request, response); }); set_floor_enable_srv_ = node_->create_service( - "set_floor_enable", [this](const std::shared_ptr request_header, + camera_name_ + "/" + "set_floor_enable", [this](const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { setFloorEnableCallback(request_header, request, response); }); set_laser_enable_srv_ = node_->create_service( - "set_laser_enable", [this](const std::shared_ptr request_header, + camera_name_ + "/" + "set_laser_enable", [this](const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { setLaserEnableCallback(request_header, request, response); }); set_ldp_enable_srv_ = node_->create_service( - "set_ldp_enable", [this](const std::shared_ptr request_header, + camera_name_ + "/" + "set_ldp_enable", [this](const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { setLdpEnableCallback(request_header, request, response); }); get_ldp_status_srv_ = node_->create_service( - "get_ldp_status", [this](const std::shared_ptr request_header, + camera_name_ + "/" + "get_ldp_status", [this](const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) { (void)request_header; @@ -137,92 +137,92 @@ void OBCameraNode::setupCameraCtrlServices() { }); get_white_balance_srv_ = node_->create_service( - "get_white_balance", [this](const std::shared_ptr request, + camera_name_ + "/" + "get_white_balance", [this](const std::shared_ptr request, std::shared_ptr response) { getWhiteBalanceCallback(request, response); }); set_white_balance_srv_ = node_->create_service( - "set_white_balance", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_white_balance", [this](const std::shared_ptr request, std::shared_ptr response) { setWhiteBalanceCallback(request, response); }); get_auto_white_balance_srv_ = node_->create_service( - "get_auto_white_balance", [this](const std::shared_ptr request, + camera_name_ + "/" + "get_auto_white_balance", [this](const std::shared_ptr request, std::shared_ptr response) { getAutoWhiteBalanceCallback(request, response); }); set_auto_white_balance_srv_ = node_->create_service( - "set_auto_white_balance", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_auto_white_balance", [this](const std::shared_ptr request, std::shared_ptr response) { setAutoWhiteBalanceCallback(request, response); }); get_device_srv_ = node_->create_service( - "get_device_info", [this](const std::shared_ptr request, + camera_name_ + "/" + "get_device_info", [this](const std::shared_ptr request, std::shared_ptr response) { getDeviceInfoCallback(request, response); }); get_sdk_version_srv_ = node_->create_service( - "get_sdk_version", + camera_name_ + "/" + "get_sdk_version", [this](const std::shared_ptr request, std::shared_ptr response) { getSDKVersion(request, response); }); save_images_srv_ = node_->create_service( - "save_images", [this](const std::shared_ptr request, + camera_name_ + "/" + "save_images", [this](const std::shared_ptr request, std::shared_ptr response) { saveImageCallback(request, response); }); save_point_cloud_srv_ = node_->create_service( - "save_point_cloud", [this](const std::shared_ptr request, + camera_name_ + "/" + "save_point_cloud", [this](const std::shared_ptr request, std::shared_ptr response) { savePointCloudCallback(request, response); }); switch_ir_camera_srv_ = node_->create_service( - "switch_ir", [this](const std::shared_ptr request, + camera_name_ + "/" + "switch_ir", [this](const std::shared_ptr request, std::shared_ptr response) { switchIRCameraCallback(request, response); }); set_ir_long_exposure_srv_ = node_->create_service( - "set_ir_long_exposure", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_ir_long_exposure", [this](const std::shared_ptr request, std::shared_ptr response) { setIRLongExposureCallback(request, response); }); get_lrm_measure_distance_srv_ = node_->create_service( - "get_lrm_measure_distance", [this](const std::shared_ptr request, + camera_name_ + "/" + "get_lrm_measure_distance", [this](const std::shared_ptr request, std::shared_ptr response) { getLrmMeasureDistanceCallback(request, response); }); set_reset_timestamp_srv_ = node_->create_service( - "set_reset_timestamp", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_reset_timestamp", [this](const std::shared_ptr request, std::shared_ptr response) { setRESETTimestampCallback(request, response); }); set_interleaver_laser_sync_srv_ = node_->create_service( - "set_sync_interleaverlaser", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_sync_interleaverlaser", [this](const std::shared_ptr request, std::shared_ptr response) { setSYNCInterleaveLaserCallback(request, response); }); set_sync_host_time_srv_ = node_->create_service( - "set_sync_hosttime", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_sync_hosttime", [this](const std::shared_ptr request, std::shared_ptr response) { setSYNCHostimeCallback(request, response); }); send_service_trigger_srv_ = node_->create_service( - "send_service_trigger", [this](const std::shared_ptr request, + camera_name_ + "/" + "send_service_trigger", [this](const std::shared_ptr request, std::shared_ptr response) { sendSoftwareTriggerCallback(request, response); }); set_write_customerdata_srv_ = node_->create_service( - "set_write_customer_data", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_write_customer_data", [this](const std::shared_ptr request, std::shared_ptr response) { setWriteCustomerData(request, response); }); set_read_customerdata_srv_ = node_->create_service( - "set_read_customer_data", [this](const std::shared_ptr request, + camera_name_ + "/" + "set_read_customer_data", [this](const std::shared_ptr request, std::shared_ptr response) { setReadCustomerData(request, response); }); change_state_srv_ = node_->create_service( - "change_state", std::bind(&OBCameraNode::handleChangeStateRequest, this, + camera_name_ + "/" + "change_state", std::bind(&OBCameraNode::handleChangeStateRequest, this, std::placeholders::_1, std::placeholders::_2)); } diff --git a/orbbec_camera_py/launch/orbbec_camera_trigger.launch.xml b/orbbec_camera_py/launch/orbbec_camera_trigger.launch.xml index e14c6399..f2b3f7c7 100644 --- a/orbbec_camera_py/launch/orbbec_camera_trigger.launch.xml +++ b/orbbec_camera_py/launch/orbbec_camera_trigger.launch.xml @@ -1,10 +1,11 @@ - + + - - + +