Path::Path() {
ros::NodeHandle nh;
XmlRpc::XmlRpcValue pathCfg;
nh.getParam("/path", pathCfg);
try {
int i = 0;
do {
char pointName[256];
sprintf(pointName, "point%d", i);
if (!pathCfg.hasMember(pointName))
break;
XmlRpc::XmlRpcValue pointCfg = pathCfg[std::string(pointName)];
geometry_msgs::Point p;
if (!(pointCfg.hasMember("x") && pointCfg.hasMember("y")))
continue;
p.x = FLOAT_PARAM(pointCfg["x"]);
p.y = FLOAT_PARAM(pointCfg["y"]);
p.z = 0;
points.push_back(p);
} while ((++i) != 0);
} catch (XmlRpc::XmlRpcException& e) {
ROS_ERROR("Unable to parse path parameter. (%s)", e.getMessage().c_str());
}
}
bool DepthImageOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue ¶ms)
{
try
{
sensor_type_ = (std::string) params["sensor_type"];
if (params.hasMember("image_topic"))
image_topic_ = (std::string) params["image_topic"];
if (params.hasMember("queue_size"))
queue_size_ = (int)params["queue_size"];
readXmlParam(params, "near_clipping_plane_distance", &near_clipping_plane_distance_);
readXmlParam(params, "far_clipping_plane_distance", &far_clipping_plane_distance_);
readXmlParam(params, "shadow_threshold", &shadow_threshold_);
readXmlParam(params, "padding_scale", &padding_scale_);
readXmlParam(params, "padding_offset", &padding_offset_);
readXmlParam(params, "skip_vertical_pixels", &skip_vertical_pixels_);
readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_);
}
catch (XmlRpc::XmlRpcException &ex)
{
ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
return false;
}
return true;
}
bool PointCloudMoveitFilter::setParams(XmlRpc::XmlRpcValue ¶ms)
{
try
{
if (!params.hasMember("point_cloud_topic"))
return false;
point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
readXmlParam(params, "max_range", &max_range_);
readXmlParam(params, "padding_offset", &padding_);
readXmlParam(params, "padding_scale", &scale_);
readXmlParam(params, "point_subsample", &point_subsample_);
if (!params.hasMember("filtered_cloud_topic")) {
ROS_ERROR("filtered_cloud_topic is required");
return false;
}
else {
filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
}
if (params.hasMember("filtered_cloud_use_color")) {
use_color_ = (bool)params["filtered_cloud_use_color"];
}
if (params.hasMember("filtered_cloud_keep_organized")) {
keep_organized_ = (bool)params["filtered_cloud_keep_organized"];
}
}
catch (XmlRpc::XmlRpcException& ex)
{
ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
return false;
}
return true;
}
PulseMotorController::PulseMotorController(
XmlRpc::XmlRpcValue &pulse_motor_info)
: motor_regs_(NULL), start_count_(0), last_target_(0) {
ROS_ASSERT(pulse_motor_info.getType() == XmlRpc::XmlRpcValue::TypeStruct);
ROS_ASSERT(pulse_motor_info.hasMember("name"));
ROS_ASSERT(pulse_motor_info.hasMember("dev_filename"));
ROS_ASSERT(pulse_motor_info.hasMember("gpio_filename"));
ROS_ASSERT(pulse_motor_info.hasMember("to_meter_fraction"));
ROS_ASSERT(pulse_motor_info.hasMember("speed"));
// Open device files
name_ = (string)pulse_motor_info["name"];
string dev_file = pulse_motor_info["dev_filename"];
legal_checker_ =
SwitchGPIOPtr(new SwitchGPIO(pulse_motor_info["gpio_filename"]));
dev_fd_ = open(dev_file.c_str(), O_RDWR, 0);
if (dev_fd_ < 0) {
ROS_ERROR("Failed to open file %s for motor %s", dev_file.c_str(),
name_.c_str());
return;
}
ROS_ASSERT(sizeof(unsigned) == 4);
motor_regs_ =
(unsigned *)mmap(NULL, sizeof(unsigned) * 4, PROT_READ | PROT_WRITE,
MAP_SHARED, dev_fd_, 0);
if ((long long)motor_regs_ == -1) {
close(dev_fd_);
ROS_ERROR("Failed to create memory map for motor %s", name_.c_str());
motor_regs_ = NULL;
return;
}
// Check initial position
printf("f**k 1\n");
ROS_INFO("Motor %s suceessfully mapped file %s", name_.c_str(),
dev_file.c_str());
can_move_ = !legal_checker_->Get();
printf("f**k 2\n");
// Setup
to_meter_fraction_ = pulse_motor_info["to_meter_fraction"];
printf("f**k 3\n");
ROS_INFO("get to_meter_fraction");
if (!can_move_) ROS_WARN("Motor %s not at initial position", name_.c_str());
speed_ = (unsigned)((int)pulse_motor_info["speed"]);
printf("f**k 4\n");
motor_regs_[kCTRL] = CTRL_DIR & ~CTRL_ENABLE;
motor_regs_[kVEL] = speed_;
start_count_ = motor_regs_[kFEEDBACK];
printf("f**k 5\n");
ROS_INFO("Pulse motor %s starts with count %u", name_.c_str(), start_count_);
}
Servo::Servo(XmlRpc::XmlRpcValue& servo_info)
: channel_(-1)
, type_(RCSERVO_SERVO_DEFAULT)
, joint_name_("")
, trim_pwm_(0)
, max_pwm_(2300)
, min_pwm_(700)
, max_rot_(HALFPI)
, min_rot_(-HALFPI)
, bus_(PWM)
{
if (servo_info.hasMember("joint_name"))
joint_name_ = (std::string)servo_info["joint_name"];
if (servo_info.hasMember("channel"))
channel_ = servo_info["channel"];
if (servo_info.hasMember("type"))
{
std::string type_string = servo_info["type"];
if (type_string == "RCSERVO_KONDO_KRS78X")
type_ = RCSERVO_KONDO_KRS78X;
else if (type_string == "RCSERVO_KONDO_KRS788")
type_ = RCSERVO_KONDO_KRS788;
else if (type_string == "RCSERVO_KONDO_KRS4014")
type_ = RCSERVO_KONDO_KRS4014;
else if (type_string == "RCSERVO_HITEC_HSR8498")
type_ = RCSERVO_HITEC_HSR8498;
else if (type_string == "KONDO_ROS3030")
type_ = KONDO_ROS3030;
else
type_ = RCSERVO_SERVO_DEFAULT;
}
if (servo_info.hasMember("bus"))
{
std::string bus_string = servo_info["bus"];
if (bus_string == "COM4")
bus_ = COM4;
}
if (servo_info.hasMember("min_rotation"))
min_rot_ = (double)servo_info["min_rotation"];
if (servo_info.hasMember("max_rotation"))
max_rot_ = (double)servo_info["max_rotation"];
if (servo_info.hasMember("min_pwm"))
min_pwm_ = (int)servo_info["min_pwm"];
if (servo_info.hasMember("max_pwm"))
max_pwm_ = (int)servo_info["max_pwm"];
if (servo_info.hasMember("trim"))
trim_pwm_ = servo_info["trim"];
}
bool FTParamsInternal::getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len)
{
if(!params.hasMember(name))
{
ROS_ERROR("Expected ft_param to have '%s' element", name);
return false;
}
XmlRpc::XmlRpcValue values = params[name];
if (values.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Expected FT param '%s' to be list type", name);
return false;
}
if (values.size() != int(len))
{
ROS_ERROR("Expected FT param '%s' to have %d elements", name, len);
return false;
}
for (unsigned i=0; i<len; ++i)
{
if (values[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
{
ROS_ERROR("Expected FT param %s[%d] to be floating point type", name, i);
return false;
} else {
results[i] = static_cast<double>(values[i]);
}
}
return true;
}
void CServo::setParams(XmlRpc::XmlRpcValue params)
{
if(params.hasMember("id"))
{
id_=params["id"];
}
if(params.hasMember("max_angle_degrees"))
{
max_angle_=(double)radians(params["max_angle_degrees"]);
}
if(params.hasMember("min_angle_degrees"))
{
min_angle_=(double)radians(params["min_angle_degrees"]);
}
if(params.hasMember("max_angle_radians"))
{
max_angle_=(double)(params["max_angle_radians"]);
}
if(params.hasMember("min_angle_radians"))
{
min_angle_=(double)(params["min_angle_radians"]);
}
if(params.hasMember("max_speed"))
{
max_speed_=(double)params["max_speed"];
}
if(params.hasMember("range"))
{
range_=(double)radians(params["range"]);
}
if(params.hasMember("ticks"))
{
ticks_=params["ticks"];
}
if(params.hasMember("neutral"))
{
neutral_=params["neutral"];
}
if(params.hasMember("invert"))
{
invert_=params["invert"];
}
recalculateAngleParams();
}
void OccupancyMapUpdater::readXmlParam(XmlRpc::XmlRpcValue ¶ms, const std::string ¶m_name, double *value)
{
if (params.hasMember(param_name))
{
if (params[param_name].getType() == XmlRpc::XmlRpcValue::TypeInt)
*value = (int) params[param_name];
else
*value = (double) params[param_name];
}
}
bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, double& value)
{
if (!config.hasMember(key))
{
return false;
}
XmlRpc::XmlRpcValue param = config[key];
if (param.getType() != XmlRpc::XmlRpcValue::TypeDouble)
{
return false;
}
value = param;
return true;
}
bool PublishedRigidBody::validateParam(XmlRpc::XmlRpcValue &config_node, const std::string &name)
{
if (!config_node.hasMember(name))
{
return false;
}
if (config_node[name].getType() != XmlRpc::XmlRpcValue::TypeString)
{
return false;
}
return true;
}
bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue ¶ms)
{
try
{
if (!params.hasMember("point_cloud_topic"))
return false;
point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
readXmlParam(params, "max_range", &max_range_);
readXmlParam(params, "padding_offset", &padding_);
readXmlParam(params, "padding_scale", &scale_);
readXmlParam(params, "point_subsample", &point_subsample_);
if (params.hasMember("filtered_cloud_topic"))
filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
}
catch (XmlRpc::XmlRpcException &ex)
{
ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
return false;
}
return true;
}
bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, std::vector<double>& double_array)
{
if (!config.hasMember(key))
{
ROS_ERROR("XmlRpcValue does not contain key %s.", key.c_str());
return false;
}
XmlRpc::XmlRpcValue d_array_xml = config[key];
if (d_array_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("XmlRpcValue is not of type array.");
return false;
}
double_array.clear();
for (int i=0; i<d_array_xml.size(); ++i)
{
if (d_array_xml[i].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
d_array_xml[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
{
ROS_ERROR("XmlRpcValue is neither a double nor a integer array.");
return false;
}
double value = 0.0;
if (d_array_xml[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
{
value = static_cast<double>(static_cast<int>(d_array_xml[i]));
}
else
{
value = static_cast<double>(d_array_xml[i]);
}
double_array.push_back(value);
}
return true;
}
bool VisualizationBase::readParameters(XmlRpc::XmlRpcValue& config)
{
if (config.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("A filter configuration must be a map with fields name, type, and params.");
return false;
}
// Check to see if we have parameters in our list.
if (config.hasMember("params")) {
XmlRpc::XmlRpcValue params = config["params"];
if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("Params must be a map.");
return false;
} else {
for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) {
ROS_DEBUG("Loading param %s\n", it->first.c_str());
parameters_[it->first] = it->second;
}
}
}
return true;
}
static void readXmlParam(XmlRpc::XmlRpcValue ¶ms, const std::string ¶m_name, unsigned int *value)
{
if (params.hasMember(param_name))
*value = (int) params[param_name];
}
void OccupancyMapUpdater::readXmlParam(XmlRpc::XmlRpcValue ¶ms, const std::string ¶m_name, unsigned int *value)
{
if (params.hasMember(param_name))
*value = (int) params[param_name];
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "standalone_complexed_nodelet");
ros::NodeHandle private_nh("~");
ros::NodeHandle nh;
nodelet::Loader manager(false); // Don't bring up the manager ROS API
nodelet::V_string my_argv;
std::vector<std::string> candidate_root;
candidate_root.push_back("nodelets");
int candidate_num;
private_nh.param("candidate_num", candidate_num, 100);
for (size_t i = 0; i < candidate_num; i++) {
candidate_root.push_back((boost::format("nodelets_%lu") % i).str());
}
for (size_t i_candidate = 0; i_candidate < candidate_root.size(); i_candidate++) {
std::string root_name = candidate_root[i_candidate];
if (private_nh.hasParam(root_name)) {
XmlRpc::XmlRpcValue nodelets_values;
private_nh.param(root_name, nodelets_values, nodelets_values);
if (nodelets_values.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t i_nodelet = 0; i_nodelet < nodelets_values.size(); i_nodelet++) {
ROS_INFO("i_nodelet %lu", i_nodelet);
XmlRpc::XmlRpcValue onenodelet_param = nodelets_values[i_nodelet];
if (onenodelet_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
std::string name, type;
nodelet::M_string remappings;
if (onenodelet_param.hasMember("if") && !(bool)onenodelet_param["if"]) {
continue;
}
if (onenodelet_param.hasMember("unless") && (bool)onenodelet_param["unless"]) {
continue;
}
if (onenodelet_param.hasMember("name")) {
name = nh.resolveName((std::string)onenodelet_param["name"]);
}
else {
ROS_FATAL("element ~nodelets should have name field");
return 1;
}
if (onenodelet_param.hasMember("type")) {
type = (std::string)onenodelet_param["type"];
}
else {
ROS_FATAL("element ~nodelets should have type field");
return 1;
}
if (onenodelet_param.hasMember("remappings")) {
XmlRpc::XmlRpcValue remappings_params
= onenodelet_param["remappings"];
if (remappings_params.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t remappings_i = 0; remappings_i < remappings_params.size(); remappings_i++) {
XmlRpc::XmlRpcValue remapping_element_param = remappings_params[remappings_i];
if (remapping_element_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
if (remapping_element_param.hasMember("from") && remapping_element_param.hasMember("to")) {
std::string from = (std::string)remapping_element_param["from"];
std::string to = (std::string)remapping_element_param["to"];
if (from.size() > 0 && from[0] == '~') {
ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
from = nodelet_private_nh.resolveName(from.substr(1, from.size() - 1));
}
else {
ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
from = nodelet_nh.resolveName(from);
}
if (to.size() > 0 && to[0] == '~') {
ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
to = nodelet_private_nh.resolveName(to.substr(1, to.size() - 1));
}
else {
ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
to = nodelet_nh.resolveName(to);
}
ROS_INFO("remapping: %s => %s", from.c_str(), to.c_str());
remappings[from] = to;
}
else {
ROS_FATAL("remappings parameter requires from and to fields");
return 1;
}
}
else {
ROS_FATAL("remappings should be an array");
return 1;
}
}
}
else {
ROS_FATAL("remappings should be an array");
return 1;
}
}
if (onenodelet_param.hasMember("params")) {
if (onenodelet_param["params"].getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_FATAL("params parameter must be an array");
return 1;
}
XmlRpc::XmlRpcValue values;
for (int params_i = 0; params_i < onenodelet_param["params"].size(); ++params_i) {
XmlRpc::XmlRpcValue oneparam = onenodelet_param["params"][params_i];
if (!(oneparam.getType() == XmlRpc::XmlRpcValue::TypeStruct &&
oneparam.hasMember("name") && oneparam.hasMember("value"))) {
ROS_FATAL("each 'params' element must be a struct which contains 'name' and 'value' fields.");
return 1;
}
std::string key = oneparam["name"];
values[key] = oneparam["value"];
ROS_INFO_STREAM("setparam: " << name << "/" << key << " => " << values[key]);
}
ros::param::set(name, values);
}
// Done reading parmaeter for one nodelet
if (!manager.load(name, type, remappings, my_argv)) {
ROS_ERROR("Failed to load nodelet [%s -- %s]", name.c_str(), type.c_str());
}
else {
ROS_INFO("Succeeded to load nodelet [%s -- %s]", name.c_str(), type.c_str());
}
}
else {
ROS_FATAL("element ~nodelets should be a dictionay");
return 1;
}
}
}
else {
ROS_FATAL("~nodelets should be a list");
return 1;
}
}
}
ROS_INFO("done reading parmaeters");
std::vector<std::string> loaded_nodelets = manager.listLoadedNodelets();
ROS_INFO("loaded nodelets: %lu", loaded_nodelets.size());
for (size_t i = 0; i < loaded_nodelets.size(); i++) {
ROS_INFO("loaded nodelet: %s", loaded_nodelets[i].c_str());
}
ros::spin();
return 0;
}
SolverConfiguration::SolverConfiguration(XmlRpc::XmlRpcValue& conf) {
if (conf.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_FATAL("solver: bad configuration, expecting structure.");
ROS_BREAK();
}
if (!conf.hasMember("base_link_frame_id")
|| conf["base_link_frame_id"].getType()
!= XmlRpc::XmlRpcValue::TypeString) {
std::cerr << conf["type"].getType() << std::endl;
ROS_FATAL("solver: 'base_link_frame_id' field undefined or not a string");
ROS_BREAK();
}
base_link_frame_id_ = static_cast<std::string &>(conf["base_link_frame_id"]);
if (!conf.hasMember("global_frame_id")
|| conf["global_frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("solver: 'global_frame' field undefined or not a string");
ROS_BREAK();
}
global_frame_ = static_cast<std::string &>(conf["global_frame_id"]);
if (!conf.hasMember("pose_window_length")
|| conf["pose_window_length"].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
ROS_FATAL("solver: 'pose_window_length' field undefined or not a double");
ROS_BREAK();
}
pose_window_length_ = static_cast<double>(conf["pose_window_length"]);
if (pose_window_length_ < 0) {
ROS_FATAL("solver: 'pose_window_length' field must be greater than zero");
ROS_BREAK();
}
if (!conf.hasMember("dead_reckoning")
|| conf["dead_reckoning"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("solver: 'dead_reckoning' field undefined or not a boolean");
ROS_BREAK();
}
dead_reckoning_ = static_cast<bool>(conf["dead_reckoning"]);
if (!conf.hasMember("frequency")
|| conf["frequency"].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
ROS_FATAL("solver: 'frequency' field undefined or not a double");
ROS_BREAK();
}
frequency_ = static_cast<double>(conf["frequency"]);
if (!conf.hasMember("n_gauss_newton_iterations")
|| conf["n_gauss_newton_iterations"].getType()
!= XmlRpc::XmlRpcValue::TypeInt) {
ROS_FATAL(
"solver: 'n_gauss_newton_iterations' field undefined or not an integer");
ROS_BREAK();
}
n_gauss_newton_iterations_ =
static_cast<int>(conf["n_gauss_newton_iterations"]);
if (!conf.hasMember("low_level_logging")
|| conf["low_level_logging"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("solver: 'low_level_logging' field undefined or not a boolean");
ROS_BREAK();
}
low_level_logging_ = static_cast<bool>(conf["low_level_logging"]);
}
SensorConfiguration::SensorConfiguration(std::string name,
XmlRpc::XmlRpcValue& conf) :
name_(name), covariance_(NULL) {
if (conf.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_FATAL("sensor %s: bad sensor configuration, expecting structure.",
name.c_str());
ROS_BREAK();
}
// set sensor type
if (!conf.hasMember("type")
|| conf["type"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'type' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
std::string type = static_cast<std::string &>(conf["type"]);
if (type == "AbsolutePosition") {
type_ = ROAMestimation::AbsolutePosition;
} else if (type == "LinearVelocity") {
type_ = ROAMestimation::LinearVelocity;
} else if (type == "AngularVelocity") {
type_ = ROAMestimation::AngularVelocity;
} else if (type == "LinearAcceleration") {
type_ = ROAMestimation::LinearAcceleration;
} else if (type == "AckermannOdometer") {
type_ = ROAMestimation::AckermannOdometer;
} else if (type == "TriskarOdometer") {
type_ = ROAMestimation::TriskarOdometer;
} else if (type == "DifferentialDriveOdometer") {
type_ = ROAMestimation::DifferentialDriveOdometer;
} else if (type == "GenericOdometer") {
type_ = ROAMestimation::GenericOdometer;
} else if (type == "VectorField") {
type_ = ROAMestimation::VectorField;
} else if (type == "VectorFieldAsCompass") {
type_ = ROAMestimation::VectorFieldAsCompass;
} else if (type == "FixedFeaturePosition") {
type_ = ROAMestimation::FixedFeaturePosition;
} else if (type == "FixedFeaturePose") {
type_ = ROAMestimation::FixedFeaturePose;
} else if (type == "PlanarConstraint") {
type_ = ROAMestimation::PlanarConstraint;
}else if (type == "IMUHandler") {
type_ = ROAMestimation::IMUHandler;
}else {
ROS_FATAL("sensor %s: unknown sensor type '%s'", name.c_str(),
type.c_str());
ROS_BREAK();
}
// set master
if (!conf.hasMember("is_master")
|| conf["is_master"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("sensor %s: 'is_master' field undefined or not a boolean",
name.c_str());
ROS_BREAK();
}
is_master_ = (bool &) conf["is_master"];
// set frame id
if (!conf.hasMember("frame_id")
|| conf["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'frame_id' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
frame_id_ = static_cast<std::string &>(conf["frame_id"]);
// set topic
if (!conf.hasMember("topic")
|| conf["topic"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'topic' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
topic_ = static_cast<std::string &>(conf["topic"]);
// set topic type
if (!conf.hasMember("topic_type")
|| conf["topic_type"].getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_FATAL("sensor %s: 'topic_type' field undefined or not a string",
name.c_str());
ROS_BREAK();
}
topic_type_ = static_cast<std::string &>(conf["topic_type"]);
// set use header tstamp
if (!conf.hasMember("use_header_stamp")
|| conf["use_header_stamp"].getType()
!= XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("sensor %s: 'use_header_stamp' field undefined or not a boolean",
name.c_str());
ROS_BREAK();
}
use_header_stamp_ = static_cast<bool>(conf["use_header_stamp"]);
// set whatever this sensor has static covariance matrix
if (!conf.hasMember("static_covariance")
|| conf["static_covariance"].getType()
!= XmlRpc::XmlRpcValue::TypeBoolean) {
ROS_FATAL("sensor %s: 'static_covariance' field undefined or not a boolean",
name.c_str());
ROS_BREAK();
}
static_covariance_ = static_cast<bool>(conf["static_covariance"]);
if (static_covariance_ == true) {
if (conf.hasMember("covariance")) {
covariance_ = new Eigen::MatrixXd();
if (conf["covariance"].getType() != XmlRpc::XmlRpcValue::TypeArray
|| !XmlRpcValueToEigenXd(conf["covariance"], covariance_)) {
ROS_FATAL("sensor %s: malformed 'covariance' field",
name.c_str());
ROS_BREAK();
}
} else {
ROS_INFO("sensor %s: ignoring static covariance", name.c_str());
}
}
// load parameters
if (conf.hasMember("parameters")) {
XmlRpc::XmlRpcValue & params = conf["parameters"];
if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_FATAL("sensor %s: malformed parameters section, expecting structure",
name.c_str());
ROS_BREAK();
}
XmlRpc::XmlRpcValue::iterator it;
for (it = params.begin(); it != params.end(); ++it) {
Eigen::VectorXd *tmp = new Eigen::VectorXd;
if (it->second.getType() != XmlRpc::XmlRpcValue::TypeArray
|| !XmlRpcValueToEigenXd(it->second, tmp)) {
ROS_FATAL("sensor %s: malformed array at parameter '%s'", name.c_str(),
it->first.c_str());
ROS_BREAK();
}
// this check is needed since at the present we decide parameter type
// depending on its size. 4 -> Quaternion, 3-> Euclidean3D, etc
if (tmp->rows() != 1 && tmp->rows() != 3 && tmp->rows() != 4
&& tmp->rows() != 9) {
ROS_FATAL("sensor %s: parameter '%s' has a wrong size: %ld",
name.c_str(), it->first.c_str(), tmp->rows());
ROS_BREAK();
}
parameters_[it->first] = tmp;
}
}
}
bool createLinksFromParams(XmlRpc::XmlRpcValue ¶ms, std::vector<LinkInfo> &links)
{
double default_padding, default_scale;
if(params.hasMember("self_see_default_padding"))
default_padding = double(params["self_see_default_padding"]);
else
default_padding = 0.01;
if(params.hasMember("self_see_default_scale"))
default_scale = double(params["self_see_default_scale"]);
else
default_scale = 1.0;
std::string link_names;
if (!params.hasMember("self_see_links"))
{
ROS_WARN ("No links specified for self filtering.");
}
else
{
XmlRpc::XmlRpcValue ssl_vals = params["self_see_links"];
if (ssl_vals.getType () != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_WARN ("Self see links need to be an array");
}
else
{
if (ssl_vals.size () == 0)
{
ROS_WARN ("No values in self see links array");
}
else
{
for (int i = 0; i < ssl_vals.size (); ++i)
{
robot_self_filter::LinkInfo li;
if (ssl_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_WARN ("Self see links entry %d is not a structure. Stopping processing of self see links", i);
return false;
}
if (!ssl_vals[i].hasMember ("name"))
{
ROS_WARN ("Self see links entry %d has no name. Stopping processing of self see links", i);
return false;
}
li.name = std::string (ssl_vals[i]["name"]);
if (!ssl_vals[i].hasMember ("padding"))
{
ROS_DEBUG ("Self see links entry %d has no padding. Assuming default padding of %g", i, default_padding);
li.padding = default_padding;
}
else
{
li.padding = ssl_vals[i]["padding"];
}
if (!ssl_vals[i].hasMember ("scale"))
{
ROS_DEBUG ("Self see links entry %d has no scale. Assuming default scale of %g", i, default_scale);
li.scale = default_scale;
}
else
{
li.scale = ssl_vals[i]["scale"];
}
links.push_back (li);
}
}
}
}
return true;
}
/**
Constructor
*/
CButGraspingControls::CButGraspingControls(wxWindow *parent, const wxString& title, rviz::WindowManagerInterface * wmi )
: wxPanel( parent, wxID_ANY, wxDefaultPosition, wxSize(280, 180), wxTAB_TRAVERSAL, title)
, m_wmi( wmi )
{
parent_ = parent;
ros::NodeHandle nh("~");
ros::param::param<double>("~abs_max_force", abs_max_force_ , ABS_MAX_FORCE);
ros::param::param<bool>("~wait_for_allow", wait_for_allow_ , WAIT_FOR_ALLOW);
XmlRpc::XmlRpcValue pres;
if (nh.getParam("presets",pres)) {
ROS_ASSERT(pres.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_INFO("%d presets for assisted grasping plugin.",pres.size());
for (int i=0; i < pres.size(); i++) {
Preset pr;
XmlRpc::XmlRpcValue xpr = pres[i];
if (xpr.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("Wrong syntax in YAML config.");
continue;
}
// read the name
if (!xpr.hasMember("name")) {
ROS_ERROR("Preset doesn't have 'name' property defined.");
continue;
} else {
XmlRpc::XmlRpcValue name = xpr["name"];
std::string tmp = static_cast<std::string>(name);
pr.name = tmp;
std::cout << tmp << " ";
}
// read the time
if (!xpr.hasMember("time")) {
ROS_ERROR("Preset doesn't have 'time' property defined.");
continue;
} else {
XmlRpc::XmlRpcValue time = xpr["time"];
double tmp = static_cast<double>(time);
pr.time = ros::Duration(tmp);
std::cout << tmp << " ";
}
// read the forces
if (!xpr.hasMember("forces")) {
ROS_ERROR("Preset doesn't have 'forces' property defined.");
continue;
} else {
XmlRpc::XmlRpcValue forces = xpr["forces"];
if (forces.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (int i=0; i < forces.size(); i++) {
XmlRpc::XmlRpcValue f = forces[i];
double tmp = static_cast<double>(f);
pr.forces.push_back(tmp);
}
} else {
ROS_ERROR("Property 'forces' is defined in bad way.");
continue;
}
} // else forces
// read the positions
if (!xpr.hasMember("target_pos")) {
ROS_ERROR("Preset doesn't have 'target_pos' property defined.");
continue;
} else {
XmlRpc::XmlRpcValue target_pos = xpr["target_pos"];
if (target_pos.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (int i=0; i < target_pos.size(); i++) {
XmlRpc::XmlRpcValue p = target_pos[i];
double tmp = static_cast<double>(p);
pr.target_pos.push_back(tmp);
}
} else {
ROS_ERROR("Property 'target_pos' is defined in bad way.");
continue;
}
} // else target_pos
// TODO test number of forces / positions !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
presets_.push_back(pr);
} // for around all presets
} else {
ROS_ERROR("Could not get presets for grasping, using defaults.");
Preset pr;
pr.name = "Empty bottle";
pr.forces.resize(6);
for(int i=0; i < 6; i++) pr.forces[i] = 300.0;
pr.target_pos.resize(7);
pr.target_pos[0] = 0.0;
pr.target_pos[1] = 0.0;
pr.target_pos[2] = 1.0472;
pr.target_pos[3] = 0.0;
pr.target_pos[4] = 1.0472;
pr.target_pos[5] = 0.0;
pr.target_pos[6] = 1.0472;
pr.time = ros::Duration(5.0);
presets_.push_back(pr);
}
wxArrayString prt;
for(unsigned int i=0; i < presets_.size(); i++)
prt.Add(wxString::FromAscii(presets_[i].name.c_str()));
presets_choice_ = new wxChoice(this, ID_CHOICE,wxDefaultPosition,wxDefaultSize,prt);
buttons_["grasp"] = new wxButton(this, ID_BUTTON_GRASP, wxT("Grasp"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
buttons_["stop"] = new wxButton(this, ID_BUTTON_STOP, wxT("Stop"),wxDefaultPosition,wxDefaultSize,wxBU_EXACTFIT);
buttons_["stop"]->SetForegroundColour (wxColour (255, 255, 255));
buttons_["stop"]->SetBackgroundColour (wxColour (255, 108, 108));
m_text_status_ = new wxStaticText(this, -1, wxT("status: idle"));
stringstream ss (stringstream::in | stringstream::out);
std::vector<double> f = presets_[presets_choice_->GetSelection()].forces;
double max_f = 0.0;
for(unsigned int i=0; i < f.size(); i++) {
if (f[i] > max_f) max_f = f[i];
}
m_text_max_force_ = new wxStaticText(this, -1, wxT("Maximum force"));
max_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE,(int)max_f,0,abs_max_force_,wxDefaultPosition,wxDefaultSize,wxSL_LABELS);
max_force_slider_->Enable(false);
finger1_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE_1,0.0,0,abs_max_force_);
finger2_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE_2,0.0,0,abs_max_force_);
finger3_force_slider_ = new wxSlider(this,ID_SLIDER_FORCE_3,0.0,0,abs_max_force_);
tactile_data_.resize(6);
wxSizer *hsizer_buttons = new wxBoxSizer(wxHORIZONTAL);
wxSizer *vsizer_glob = new wxBoxSizer(wxVERTICAL);
hsizer_buttons->Add(buttons_["grasp"], ID_BUTTON_GRASP);
hsizer_buttons->Add(buttons_["stop"], ID_BUTTON_STOP);
//wxSizer *vsizer = new wxBoxSizer(wxVERTICAL); // top sizer
wxSizer *vsizer_top = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Object grasping"));
vsizer_top->Add(hsizer_buttons,1,wxEXPAND);
vsizer_top->Add(m_text_status_);
vsizer_top->Add(presets_choice_);
vsizer_top->Add(m_text_max_force_);
vsizer_top->Add(max_force_slider_,1,wxEXPAND);
wxSizer *vsizer_sliders = new wxStaticBoxSizer(wxVERTICAL,this,wxT("Force feedback"));
vsizer_sliders->Add(finger1_force_slider_,1,wxEXPAND);
vsizer_sliders->Add(finger2_force_slider_,1,wxEXPAND);
vsizer_sliders->Add(finger3_force_slider_,1,wxEXPAND);
//vsizer->Add(vsizer_sliders,1,wxEXPAND);
finger1_force_slider_->Enable(false);
finger2_force_slider_->Enable(false);
finger3_force_slider_->Enable(false);
setButton("stop",false);
grasping_allowed_ = false;
if (wait_for_allow_) {
DisableControls();
} else {
EnableControls();
}
/*Connect(ID_SLIDER_FORCE, wxEVT_SCROLL_THUMBRELEASE,
wxCommandEventHandler(CButGraspingControls::OnMaxForceSlider));*/
Connect(ID_CHOICE, wxEVT_COMMAND_CHOICE_SELECTED,
wxCommandEventHandler(CButGraspingControls::OnChoice));
vsizer_glob->Add(vsizer_top,1,wxEXPAND|wxHORIZONTAL);
vsizer_glob->Add(vsizer_sliders,1,wxEXPAND|wxHORIZONTAL);
//this->SetSizerAndFit(vsizer_top);
vsizer_glob->SetSizeHints(this);
this->SetSizerAndFit(vsizer_glob);
as_client_ = new grasping_action_client(ACT_GRASP,true);
//ros::NodeHandle nh;
tactile_data_received_ = false;
stop_gui_thread_ = false;
t_gui_update = boost::thread(&CButGraspingControls::GuiUpdateThread,this);
//gui_update_timer_ = nh.createTimer(ros::Duration(0.5),&CButGraspingControls::timerCallback,this);
tact_sub_ = nh.subscribe("/sdh_controller/tactile_data_filtered", 10, &CButGraspingControls::TactileDataCallback,this);
service_grasping_allow_ = nh.advertiseService(SRV_ALLOW,&CButGraspingControls::GraspingAllow,this);
}