Skip to content

Commit

Permalink
Merge pull request #155 from mdhorn/system-call-fix
Browse files Browse the repository at this point in the history 
Clean up system process calls
  • Loading branch information
@mdhorn
mdhorn authored Nov 19, 2016
2 parents ab26677 + 52cad2c commit fc64408
Show file tree
Hide file tree
Showing 4 changed files with 148 additions and 118 deletions.
7 changes: 7 additions & 0 deletions realsense_camera/include/realsense_camera/base_nodelet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,10 @@
#include <tf2_ros/static_transform_broadcaster.h>
#include <realsense_camera/constants.h>

#include <queue>
#include <unistd.h>
#include <signal.h>

namespace realsense_camera
{
class BaseNodelet: public nodelet::Nodelet
Expand Down Expand Up @@ -142,6 +146,8 @@ namespace realsense_camera
};
std::vector<CameraOptions> camera_options_;

std::queue<pid_t> system_proc_groups_;

// Member Functions.
virtual void getParameters();
virtual bool connectToCamera();
Expand All @@ -165,6 +171,7 @@ namespace realsense_camera
virtual void publishStaticTransforms();
virtual void checkError();
virtual bool checkForSubscriber();
virtual void wrappedSystem(std::vector<std::string> string_argv);
};
}
#endif
55 changes: 55 additions & 0 deletions realsense_camera/src/base_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,13 @@ namespace realsense_camera
checkError();
}

// Kill all old system progress groups
while (! system_proc_groups_.empty())
{
killpg(system_proc_groups_.front(), SIGHUP);
system_proc_groups_.pop();
}

ROS_INFO_STREAM(nodelet_name_ << " - Stopping...");
if (! ros::isShuttingDown())
{
Expand Down Expand Up @@ -1109,4 +1116,52 @@ namespace realsense_camera
ros::shutdown();
}
}

void BaseNodelet::wrappedSystem(std::vector<std::string> string_argv)
{
pid_t pid;

// Convert the args to char * const * for exec
char * argv[string_argv.size() + 1];

for(size_t i = 0; i < string_argv.size(); ++i)
{
argv[i] = const_cast<char*>(string_argv[i].c_str());
}
argv[string_argv.size()] = NULL;

pid = fork();

if (pid == -1)
{ // failed to fork
ROS_WARN_STREAM(nodelet_name_ <<
" - Failed to set dynamic_reconfigure dc preset via system:"
<< strerror(errno));
}
else if (pid == 0)
{ // child process
// set to it's own process group
setpgid(getpid(), getpid());

// sleep for 1 second to ensure previous calls are completed
sleep(1);
// environ is the current environment from <unistd.h>
execvpe(argv[0], argv, environ);

_exit(EXIT_FAILURE); // exec never returns
}
else
{ // parent process
// Save the progress pid (process group)
system_proc_groups_.push(pid);

// If more than 10, process the oldest now
if (system_proc_groups_.size() > 10)
{
killpg(system_proc_groups_.front(), SIGHUP);
system_proc_groups_.pop();
}
// do not wait as this is the main thread
}
}
} // end namespace
102 changes: 43 additions & 59 deletions realsense_camera/src/r200_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,24 +125,16 @@ namespace realsense_camera
// There is not a C++ API for dynamic reconfig so we need to use a system call
// Adding the sleep to ensure the current callback can end before we
// attempt the next callback from the system call.
std::string system_cmd = "(sleep 1 ; rosrun dynamic_reconfigure dynparam set "
+ nodelet_name_ + " r200_dc_preset " + std::to_string(preset) + ")&";

int status = std::system(system_cmd.c_str());
if (status < 0)
{
ROS_WARN_STREAM(nodelet_name_ <<
" - Failed to set dynamic_reconfigure dc preset via system:"
<< strerror(errno));
}
else
{
if (!WIFEXITED(status))
{
ROS_WARN_STREAM(nodelet_name_ <<
" - Failed to set dynamic_reconfigure dc preset via system");
}
}
std::vector<std::string> argv;
argv.push_back("rosrun");
argv.push_back("dynamic_reconfigure");
argv.push_back("dynparam");
argv.push_back("set");
argv.push_back(nodelet_name_);
argv.push_back("r200_dc_preset");
argv.push_back(std::to_string(preset));

wrappedSystem(argv);
}

/*
Expand All @@ -152,95 +144,87 @@ namespace realsense_camera
*/
std::string R200Nodelet::setDynamicReconfigDepthControlIndividuals()
{
std::string current_param;
std::string current_dc;
std::string option_value;

// There is not a C++ API for dynamic reconfig so we need to use a system call
// Adding the sleep to ensure the current callback can end before we
// attempt the next callback from the system call.
std::string system_cmd =
"(sleep 1 ; rosrun dynamic_reconfigure dynparam set ";
system_cmd += nodelet_name_ + " " +
"\"{" +
"'r200_dc_estimate_median_decrement':";
std::vector<std::string> argv;
argv.push_back("rosrun");
argv.push_back("dynamic_reconfigure");
argv.push_back("dynparam");
argv.push_back("set");
argv.push_back(nodelet_name_);

current_param = "{";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_DECREMENT, 0)));
current_param += "'r200_dc_estimate_median_decrement':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_estimate_median_increment':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_ESTIMATE_MEDIAN_INCREMENT, 0)));
current_param += "'r200_dc_estimate_median_increment':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_median_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_MEDIAN_THRESHOLD, 0)));
current_param += "'r200_dc_median_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_score_minimum_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_SCORE_MINIMUM_THRESHOLD, 0)));
current_param += "'r200_dc_score_minimum_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_score_maximum_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_SCORE_MAXIMUM_THRESHOLD, 0)));
current_param += "'r200_dc_score_maximum_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_texture_count_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_COUNT_THRESHOLD, 0)));
current_param += "'r200_dc_texture_count_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_texture_difference_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_TEXTURE_DIFFERENCE_THRESHOLD, 0)));
current_param += "'r200_dc_texture_difference_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_second_peak_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_SECOND_PEAK_THRESHOLD, 0)));
current_param += "'r200_dc_second_peak_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_neighbor_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_NEIGHBOR_THRESHOLD, 0)));
current_param += "'r200_dc_neighbor_threshold':" + option_value + ", ";
current_dc += option_value + ":";
system_cmd += option_value +
", 'r200_dc_lr_threshold':";

option_value =
std::to_string(static_cast<uint32_t>(rs_get_device_option(rs_device_,
RS_OPTION_R200_DEPTH_CONTROL_LR_THRESHOLD, 0)));
current_param += "'r200_dc_lr_threshold':" + option_value + "}";
current_dc += option_value;
system_cmd += option_value +
"}\")&";

ROS_DEBUG_STREAM(nodelet_name_ << " - Setting DC: " << system_cmd);
ROS_DEBUG_STREAM(nodelet_name_ << " - Setting DC: " << current_param);

int status = std::system(system_cmd.c_str());
if (status < 0)
{
ROS_WARN_STREAM(nodelet_name_ <<
" - Failed to set dynamic_reconfigure dc manual via system:"
<< strerror(errno));
}
else
{
if (!WIFEXITED(status))
{
ROS_WARN_STREAM(nodelet_name_ <<
" - Failed to set dynamic_reconfigure dc manual via system");
}
}
argv.push_back(current_param);

wrappedSystem(argv);

return current_dc;
}
Expand Down
Loading

0 comments on commit fc64408

Please sign in to comment.