Skip to content

Commit

Permalink
triggered manual cppcheck with highest checks, and fixed most of them
Browse files Browse the repository at this point in the history
  • Loading branch information
SamerKhshiboun committed Jun 26, 2024
1 parent 598756b commit 656c77e
Show file tree
Hide file tree
Showing 7 changed files with 16 additions and 25 deletions.
5 changes: 2 additions & 3 deletions realsense2_camera/include/base_realsense_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -95,9 +95,8 @@ namespace realsense2_camera
~SyncedImuPublisher();
void Pause(); // Pause sending messages. All messages from now on are saved in queue.
void Resume(); // Send all pending messages and allow sending future messages.
void Publish(sensor_msgs::msg::Imu msg); //either send or hold message.
void Publish(sensor_msgs::msg::Imu imu_msg); //either send or hold message.
size_t getNumSubscribers();
void Enable(bool is_enabled) {_is_enabled=is_enabled;};

private:
void PublishPendingMessages();
Expand Down Expand Up @@ -225,7 +224,7 @@ namespace realsense2_camera
void publishStaticTransforms();
void startDynamicTf();
void publishDynamicTransforms();
void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);
void publishPointCloud(rs2::points pc, const rclcpp::Time& t, const rs2::frameset& frameset);
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;
IMUInfo getImuInfo(const rs2::stream_profile& profile);
void initializeFormatsMaps();
Expand Down
5 changes: 2 additions & 3 deletions realsense2_camera/src/base_realsense_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -815,7 +815,6 @@ void BaseRealSenseNode::updateExtrinsicsCalibData(const rs2::video_stream_profil
void BaseRealSenseNode::SetBaseStream()
{
const std::vector<stream_index_pair> base_stream_priority = {DEPTH};
std::set<stream_index_pair> checked_sips;
std::map<stream_index_pair, rs2::stream_profile> available_profiles;
for(auto&& sensor : _available_ros_sensors)
{
Expand All @@ -831,7 +830,7 @@ void BaseRealSenseNode::SetBaseStream()
std::vector<stream_index_pair>::const_iterator base_stream(base_stream_priority.begin());
while((base_stream != base_stream_priority.end()) && (available_profiles.find(*base_stream) == available_profiles.end()))
{
base_stream++;
++base_stream;
}
if (base_stream == base_stream_priority.end())
{
Expand Down Expand Up @@ -1120,7 +1119,7 @@ void BaseRealSenseNode::publishRGBD(
auto depth_camera_info = _camera_info.at(DEPTH);
msg->depth_camera_info = depth_camera_info;

realsense2_camera_msgs::msg::RGBD *msg_address = msg.get();
const realsense2_camera_msgs::msg::RGBD *msg_address = msg.get();
_rgbd_publisher->publish(std::move(msg));
ROS_DEBUG_STREAM("rgbd stream published, message address: " << std::hex << msg_address);
}
Expand Down
6 changes: 3 additions & 3 deletions realsense2_camera/src/profile_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ void ProfilesManager::registerSensorQOSParam(std::string template_name,
{
// For each pair of stream-index, Function add a QOS parameter to <params> and advertise it by <template_name>.
// parameters in <params> are dynamically being updated. If invalid they are reverted.
for (auto& sip : unique_sips)
for (const auto &sip : unique_sips)
{
std::string param_name = applyTemplateName(template_name, sip);
params[sip] = std::make_shared<std::string>(value);
Expand Down Expand Up @@ -320,7 +320,7 @@ std::string VideoProfilesManager::get_profiles_descriptions(rs2_stream stream_ty
}
}
std::stringstream descriptors_strm;
for (auto& profile_str : profiles_str)
for (const auto &profile_str : profiles_str)
{
descriptors_strm << profile_str << "\n";
}
Expand All @@ -345,7 +345,7 @@ std::string VideoProfilesManager::getProfileFormatsDescriptions(stream_index_pai
}
}
std::stringstream descriptors_strm;
for (auto& profile_format_str : profile_formats_str)
for (const auto &profile_format_str : profile_formats_str)
{
descriptors_strm << profile_format_str << "\n";
}
Expand Down
1 change: 0 additions & 1 deletion realsense2_camera/src/realsense_node_factory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,6 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list)
std::string pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT);
std::string name = dev.get_info(RS2_CAMERA_INFO_NAME);
ROS_INFO_STREAM("Device with physical ID " << pn << " was found.");
std::vector<std::string> results;
ROS_INFO_STREAM("Device with name " << name << " was found.");
std::string port_id = parseUsbPort(pn);
if (port_id.empty())
Expand Down
17 changes: 7 additions & 10 deletions realsense2_camera/src/ros_sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -355,17 +355,14 @@ bool profiles_equal(const rs2::stream_profile& a, const rs2::stream_profile& b)

bool is_profiles_in_profiles(const std::vector<stream_profile>& sub_profiles, const std::vector<stream_profile>& all_profiles)
{
for (auto& a : sub_profiles)
for (const auto &a : sub_profiles)
{
bool found_profile(false);
for (auto& b : all_profiles)
{
if (profiles_equal(a, b))
{
found_profile = true;
break;
}
}
bool found_profile = std::any_of(
all_profiles.begin(),
all_profiles.end(),
[&a](const stream_profile &b) {
return profiles_equal(a, b);
});
if (!found_profile)
{
return false;
Expand Down
2 changes: 1 addition & 1 deletion realsense2_camera/src/ros_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ static const rmw_qos_profile_t rmw_qos_profile_latched =
false
};

const rmw_qos_profile_t qos_string_to_qos(std::string str)
const rmw_qos_profile_t qos_string_to_qos(const std::string &str)
{
if (str == "UNKNOWN")
return rmw_qos_profile_unknown;
Expand Down
5 changes: 1 addition & 4 deletions realsense2_camera/src/rs_node_setup.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,6 @@ void BaseRealSenseNode::stopPublishers(const std::vector<stream_profile>& profil

void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profiles, const RosSensor& sensor)
{
const std::string module_name(create_graph_resource_name(rs2_to_ros(sensor.get_info(RS2_CAMERA_INFO_NAME))));
for (auto& profile : profiles)
{
stream_index_pair sip(profile.stream_type(), profile.stream_index());
Expand Down Expand Up @@ -264,8 +263,6 @@ void BaseRealSenseNode::startPublishers(const std::vector<stream_profile>& profi
aligned_image_raw << "~/" << "aligned_depth_to_" << stream_name << "/image_raw";
aligned_camera_info << "~/" << "aligned_depth_to_" << stream_name << "/camera_info";

std::string aligned_stream_name = "aligned_depth_to_" + stream_name;

// We can use 2 types of publishers:
// Native RCL publisher that support intra-process zero-copy comunication
// image-transport package publisher that add's a commpressed image topic if the package is installed
Expand Down Expand Up @@ -451,7 +448,7 @@ void BaseRealSenseNode::startUpdatedSensors()
if (_publish_tf)
{
std::lock_guard<std::mutex> lock_guard(_publish_tf_mutex);
for (auto &profile : wanted_profiles)
for (const auto &profile : wanted_profiles)
{
calcAndAppendTransformMsgs(profile, _base_profile);
}
Expand Down

0 comments on commit 656c77e

Please sign in to comment.