void TeleopCOB::getConfigurationFromParameters()
{
//std::map<std::string,joint_module> joint_modules; //std::vector<std::string> module_names;
if(n_.hasParam("modules"))
{
XmlRpc::XmlRpcValue modules;
ROS_DEBUG("modules found ");
n_.getParam("modules", modules);
if(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_DEBUG("modules are of type struct with size %d",(int)modules.size());
for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=modules.begin();p!=modules.end();++p)
{
std::string mod_name = p->first;
ROS_DEBUG("module name: %s",mod_name.c_str());
XmlRpc::XmlRpcValue mod_struct = p->second;
if(mod_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct)
ROS_WARN("invalid module, name: %s",mod_name.c_str());
// search for joint_name parameter in current module struct to determine which type of module
// only joint mods or wheel mods supported
// which mens that is no joint names are found, then the module is a wheel module
// TODO replace with build in find, but could not get it to work
if(!assign_joint_module(mod_name, mod_struct))
{
// add wheel module struct
ROS_DEBUG("wheel module found");
assign_base_module(mod_struct);
}
}
}
}
}
/**
* Tries to read joint module configurations from XmlRpcValue object.
* If the module is a joint module, it contains a joint name array.
* A typical joint module has the following configuration structure:
* struct {
* joint_names: ['head_pan_joint','head_tilt_joint'],
* joint_step: 0.075
* }
* @param mod_struct configuration object struct
* @return true if the configuration object hols a joint module config, else false
*/
bool TeleopCOB::assign_joint_module(std::string mod_name, XmlRpc::XmlRpcValue mod_struct)
{
// search for joint_name parameter in current module struct to determine which type of module
// only joint mods or wheel mods supported
// which mens that is no joint names are found, then the module is a wheel module
// TODO replace with build in find, but could not get it to work
bool is_joint_module = false;
joint_module tempModule;
for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps)
{
std::string par_name = ps->first;
ROS_DEBUG("par name: %s",par_name.c_str());
if(par_name.compare("joint_names")==0)
{
ROS_DEBUG("joint names found");
XmlRpc::XmlRpcValue joint_names = ps->second;
ROS_ASSERT(joint_names.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_DEBUG("joint_names.size: %d \n", joint_names.size());
for(int i=0;i<joint_names.size();i++)
{
ROS_ASSERT(joint_names[i].getType() == XmlRpc::XmlRpcValue::TypeString);
std::string s((std::string)joint_names[i]);
ROS_DEBUG("joint_name found = %s",s.c_str());
tempModule.joint_names.push_back(s);
}
// set size of other vectors according to the joint name vector
tempModule.req_joint_pos_.resize(joint_names.size());
tempModule.req_joint_vel_.resize(joint_names.size());
is_joint_module = true;
//break; // no need to continue searching if joint names are found
}else if(par_name.compare("joint_step")==0){
ROS_DEBUG("joint steps found");
XmlRpc::XmlRpcValue joint_steps = ps->second;
ROS_ASSERT(joint_steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_DEBUG("joint_steps.size: %d \n", joint_steps.size());
for(int i=0;i<joint_steps.size();i++)
{
ROS_ASSERT(joint_steps[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
double step((double)joint_steps[i]);
ROS_DEBUG("joint_step found = %f",step);
tempModule.steps.push_back(step);
}
}
}
if(is_joint_module)
{
// assign publisher
tempModule.module_publisher_ = n_.advertise<trajectory_msgs::JointTrajectory>(("/"+mod_name+"_controller/command"),1);
tempModule.module_publisher_brics_ = n_.advertise<brics_actuator::JointVelocities>(("/"+mod_name+"_controller/command_vel"),1);
// store joint module in collection
ROS_DEBUG("head module stored");
joint_modules_.insert(std::pair<std::string,joint_module>(mod_name,tempModule));
}
return is_joint_module;
}
void EmotionGenerator::getEmotionDesireRelation(std::string desireType)
{
XmlRpc::XmlRpcValue emotionParam;
std::string namespaceNode = n_->getNamespace().substr(1,n_->getNamespace().length()-1);
std::string paramServerDesire = namespaceNode + "/" + nodeName_ + "/" + desireType;
n_->getParam(paramServerDesire, emotionParam);
// ROS_INFO("Emo gen - param emotion desire relation :%s,%s,%s = %s", n_->getNamespace().c_str() ,
// nodeName_.c_str(),
// desireType.c_str(),
// paramServerDesire.c_str() );
std::map<std::string,double> mapEmotion;
ROS_INFO_COND(emotionParam.begin() != emotionParam.end() , "new desire %s :",desireType.c_str());
for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator it = emotionParam.begin(); it != emotionParam.end(); it++)
{
double emotionFactor = 0;
if((*it).second.getType() == XmlRpc::XmlRpcValue::TypeInt)
{
int value = static_cast<int>((*it).second);
ROS_INFO(" %s %i",(*it).first.c_str(), value);
emotionFactor = (double)value;
}
else if ((*it).second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
{
emotionFactor = static_cast<double>((*it).second);
ROS_INFO(" %s %5.2f",(*it).first.c_str(), emotionFactor);
}
mapEmotion[(*it).first] = emotionFactor;
if(emotionIntensities.count((*it).first) == 0)
{
//initialize this emotion
emotionIntensities[(*it).first] = 0;
}
}
if(mapEmotion.size() > 0)
emotionMatrix[desireType] = mapEmotion;
}
/**
* Tries to read base module configurations from XmlRpcValue object.
* A base module is supposed to contain vectors 3 element vectors (x,y,th)
* with max acceleration and velocity. Example:
* struct {
* max_velocity: [0.3, 0.2, 0.3],
* max_acceleration: [0.5, 0.5, 0.7]
* }
* @param mod_struct configuration object struct
* @return true no check is currently performed TODO check
*/
bool TeleopCOB::assign_base_module(XmlRpc::XmlRpcValue mod_struct)
{
for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps)
{
std::string par_name = ps->first;
ROS_DEBUG("par name: %s",par_name.c_str());
if(par_name.compare("max_velocity")==0)
{
ROS_DEBUG("max vel found");
XmlRpc::XmlRpcValue max_vel = ps->second;
ROS_ASSERT(max_vel.getType() == XmlRpc::XmlRpcValue::TypeArray);
if(max_vel.size()!=3){ROS_WARN("invalid base parameter size");}
ROS_DEBUG("max_vel.size: %d \n", max_vel.size());
for(int i=0;i<max_vel.size();i++)
{
ROS_ASSERT(max_vel[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
double val = (double)max_vel[i];
ROS_DEBUG("max vel value = %f",val);
base_module_.max_vel_.push_back(val);
}
}
else if(par_name.compare("max_acceleration")==0)
{
ROS_DEBUG("max acc found");
XmlRpc::XmlRpcValue max_acc = ps->second;
ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray);
if(max_acc.size()!=3){ROS_DEBUG("invalid base parameter size");}
ROS_DEBUG("max_acc.size: %d \n", max_acc.size());
for(int i=0;i<max_acc.size();i++)
{
ROS_ASSERT(max_acc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
double val = (double)max_acc[i];
ROS_DEBUG("max acc value = %f", val);
base_module_.max_acc_.push_back(val);
}
}
else
{
ROS_WARN("unsupported base module parameter read");
}
}
// make all the vectors the same length
// the vector size is not completely safe since only warning is
// issued if max value arrays has the wrong length
base_module_.req_vel_.resize(base_module_.max_acc_.size());
base_module_.vel_old_.resize(base_module_.max_acc_.size());
base_module_.base_publisher_ = n_.advertise<geometry_msgs::Twist>("/base_controller/command",1);
ROS_DEBUG("base module stored");
has_base_module_ = true;
return true;
}
hsvColorRange::hsvColorRange(XmlRpc::XmlRpcValue _params)
{
ROS_ASSERT(_params.getType()==XmlRpc::XmlRpcValue::TypeStruct);
for (XmlRpc::XmlRpcValue::iterator i=_params.begin(); i!=_params.end(); ++i)
{
ROS_ASSERT(i->second.getType()==XmlRpc::XmlRpcValue::TypeArray);
for(int j=0; j<i->second.size(); ++j)
{
ROS_ASSERT(i->second[j].getType()==XmlRpc::XmlRpcValue::TypeInt);
}
// printf("%s %i %i\n", i->first.c_str(), static_cast<int>(i->second[0]), static_cast<int>(i->second[1]));
if (i->first == "H") H=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
if (i->first == "S") S=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
if (i->first == "V") V=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
}
}
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;
}
void ServoInterface::loadServoCommandPriorities()
{
ros::NodeHandle nhPvt = getPrivateNodeHandle();
XmlRpc::XmlRpcValue v;
nhPvt.param("servoCommandProirities", v, v);
std::map<std::string, XmlRpc::XmlRpcValue>::iterator mapIt;
for(mapIt = v.begin(); mapIt != v.end(); mapIt++)
{
if(mapIt->second.getType() == XmlRpc::XmlRpcValue::TypeInt)
{
//add entry in priority queue and command map
priorityEntry toAdd;
toAdd.id = mapIt->first;
toAdd.priority = static_cast<int>(mapIt->second);
m_servoCommandPriorities.push_back(toAdd);
m_servoCommandMsgs[mapIt->first] = autorally_msgs::servoMSG();
} else
{
NODELET_ERROR("ServoInterface: XmlRpc servo command priorities formatted incorrectly");
}
}
std::sort(m_servoCommandPriorities.begin(),
m_servoCommandPriorities.end(),
priorityComparator());
std::vector<priorityEntry>::const_iterator vecIt;
for(vecIt = m_servoCommandPriorities.begin();
vecIt != m_servoCommandPriorities.end();
vecIt++)
{
NODELET_INFO_STREAM("ServoInterface: ServoCommand ID:Priorities:" << vecIt->id << ":" << vecIt->priority);
}
NODELET_INFO_STREAM("ServoInterface: Loaded " <<
m_servoCommandPriorities.size() << " servo commanders");
}
/**
* \brief Load the configuration from file.
* @param configurationPrefix The entry node inside the configuration YAML file.
* @return The complete image provider configuration.
*/
ImageProviderConfiguration::ConstPtr getImageProviderConfiguration () {
ImageProviderConfiguration::Ptr configuration = boost::make_shared<ImageProviderConfiguration>();
// private parameters
ros::NodeHandle privateNodeHandle ("~");
std::string configurationPrefix;
privateNodeHandle.param("cfgprefix", configurationPrefix, std::string("configuration"));
privateNodeHandle.param("startpos", configuration->startPos, 0);
privateNodeHandle.param("length", configuration->length, -1);
privateNodeHandle.param("rate", configuration->rate, 1);
privateNodeHandle.param("loop", configuration->loop, false);
privateNodeHandle.param("topicprefix", configuration->topicPrefix, std::string("cameras"));
ros::NodeHandle configNodeHandle (privateNodeHandle, configurationPrefix);
// configuration parameters (supposed to be loaded via YAML file)
if (!configNodeHandle.getParam("srcdir", configuration->sourceDir)) {
configuration->sourceDir = ".";
}
if (!configNodeHandle.getParam("filepattern", configuration->filePattern)) {
throw ImageProviderException("Parameter 'filepattern' not found");
}
try {
XmlRpc::XmlRpcValue cameraNames;
if (configNodeHandle.getParam("cameras", cameraNames)) {
std::map<std::string, XmlRpc::XmlRpcValue>::iterator nameIterator;
for (nameIterator = cameraNames.begin(); nameIterator != cameraNames.end(); ++nameIterator) {
std::string cameraName = nameIterator->first;
ROS_INFO_STREAM("Reading camera configuration for camera: " << cameraName);
CameraConfigEntry::ConstPtr cameraConfig = getCameraConfigurationEntry(configNodeHandle, cameraName);
configuration->cameraConfigs.push_back(cameraConfig);
}
}
}
catch (XmlRpc::XmlRpcException& e) {
throw ImageProviderException(std::string("Failed to retrieve camera names: ") + std::string(e.getMessage()));
}
return configuration;
}
YAML::Node XmlToYaml( XmlRpc::XmlRpcValue& xml )
{
YAML::Node yaml;
if( xml.getType() != XmlRpc::XmlRpcValue::TypeStruct ) { return yaml; }
XmlRpc::XmlRpcValue::iterator iter;
for( iter = xml.begin(); iter != xml.end(); iter++ )
{
std::string name = iter->first;
XmlRpc::XmlRpcValue payload = iter->second;
if( payload.getType() == XmlRpc::XmlRpcValue::TypeStruct )
{
yaml[name] = XmlToYaml( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeArray )
{
if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeBoolean ) )
{
std::vector<bool> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( static_cast<bool>( payload[i]) );
}
yaml[name] = s;
}
else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeInt ) )
{
std::vector<int> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( static_cast<int>( payload[i]) );
}
yaml[name] = s;
}
else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeDouble ) )
{
std::vector<double> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( ParseDouble( payload[i] ) );
}
yaml[name] = s;
}
else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeString ) )
{
std::vector<std::string> s;
for( int i = 0; i < payload.size(); i++ )
{
s.push_back( static_cast<std::string>( payload[i]) );
}
yaml[name] = s;
}
else
{
std::cerr << "Invalid array type." << std::endl;
}
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeBoolean )
{
yaml[name] = static_cast<bool>( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeInt )
{
yaml[name] = static_cast<int>( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeDouble )
{
yaml[name] = static_cast<double>( payload );
}
else if( payload.getType() == XmlRpc::XmlRpcValue::TypeString )
{
yaml[name] = static_cast<std::string>( payload );
}
else
{
std::cerr << "Unsupported conversion type." << std::endl;
continue;
}
}
return yaml;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "uav_commander");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
////////////////////////////
// Load object part types //
////////////////////////////
std::vector<std::string> robot_ids;
std::vector<Eigen::Matrix<double,4,1, Eigen::DontAlign> > desired_formation;
XmlRpc::XmlRpcValue swarm;
n_priv.getParam("swarm", swarm);
ROS_INFO_STREAM("Loading swarm of " << swarm.size() << " robots.");
std::map<std::string, XmlRpc::XmlRpcValue>::iterator i;
for (i = swarm.begin(); i != swarm.end(); i++)
{
robot_ids.push_back(i->first);
Eigen::Matrix<double,4,1, Eigen::DontAlign> pose;
double x=i->second["pose"]["x"]["value"];
double y=i->second["pose"]["y"]["value"];
double z=i->second["pose"]["z"]["value"];
double yaw=i->second["pose"]["yaw"]["value"];
pose << x, y,z, yaw;
desired_formation.push_back(pose);
}
SwarmFormationControl swarm_formation(n,robot_ids,desired_formation);
ros::Publisher goal_pub=n.advertise<geometry_msgs::PoseStamped>("swarm_goal",10);
geometry_msgs::PoseStamped goal_msg;
goal_msg.pose.position.x=0.0;
goal_msg.pose.position.y=0.0;
goal_msg.pose.position.z=1.0;
// static tf::TransformBroadcaster brr;
// tf::Transform transform;
// int increment=1;
// tf::Quaternion q;
// q.setRPY(0, 0, 0);
// transform.setRotation(q);
// transform.setOrigin( tf::Vector3(0.1, 0.1, 1.0+0.01) );
ros::AsyncSpinner spinner(4);
spinner.start();
ros::Rate loop_rate(20);
while (ros::ok())
{
// ++increment;
// transform.setOrigin( tf::Vector3(1.0, 1.0, 1.0) );
// transform.setOrigin( tf::Vector3(0.1+increment*0.001, 0.1+increment*0.001, 1.0+increment*0.001) );
// q.setRPY(0, 0, 0+increment*0.001);
// transform.setRotation(q);
// brr.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "swarm_goal"));
swarm_formation.updateCentroid();
loop_rate.sleep();
goal_msg.pose.position.x+=0.01;
goal_msg.pose.position.z+=0.001;
goal_pub.publish(goal_msg);
}
spinner.stop();
return 0;
}
void WholeBodyController::loadParameterFiles()
{
ros::NodeHandle n("~");
std::string ns = ros::this_node::getName();
XmlRpc::XmlRpcValue groups;
typedef std::map<std::string, XmlRpc::XmlRpcValue>::iterator XmlRpcIterator;
try
{
// ROBOT
n.getParam("/whole_body_controller/collision_model", groups);
for(XmlRpcIterator itrGroups = groups.begin(); itrGroups != groups.end(); ++itrGroups)
{
// COLLISION GROUP
std::vector< RobotState::CollisionBody > robot_state_group;
XmlRpc::XmlRpcValue group = itrGroups->second;
for(XmlRpcIterator itrBodies = group.begin(); itrBodies != group.end(); ++itrBodies)
{
// COLLISION BODY
XmlRpc::XmlRpcValue collisionBody = itrBodies->second;
//cout << collisionBody["name"] << endl;
RobotState::CollisionBody robotstate_collision_body;
robotstate_collision_body.fromXmlRpc(collisionBody);
// add the collision bodies to the group
robot_state_group.push_back(robotstate_collision_body);
}
// add group of collision bodies to the robot
robot_state_.robot_.groups.push_back(robot_state_group);
}
if (robot_state_.robot_.groups.size() == 0)
{
ROS_WARN("No collision model loaded");
}
XmlRpc::XmlRpcValue exclusion_groups;
n.getParam("/whole_body_controller/exlusions_collision_calculation", exclusion_groups);
for(XmlRpcIterator itrExclGr = exclusion_groups.begin(); itrExclGr != exclusion_groups.end(); ++itrExclGr)
{
XmlRpc::XmlRpcValue ExclGroup = itrExclGr->second;
for(XmlRpcIterator itrExcl = ExclGroup.begin(); itrExcl != ExclGroup.end(); ++itrExcl)
{
XmlRpc::XmlRpcValue Excl = itrExcl->second;
RobotState::Exclusion exclusion;
exclusion.fromXmlRpc(Excl);
robot_state_.exclusion_checks.checks.push_back(exclusion);
}
}
if (robot_state_.exclusion_checks.checks.size() == 0)
{
ROS_WARN("No exclusions from self-collision avoindance checks");
}
else if (robot_state_.exclusion_checks.checks.size() > 0)
{
ROS_DEBUG("Exclusions from self-collision checks are: ");
for (std::vector<RobotState::Exclusion>::iterator it = robot_state_.exclusion_checks.checks.begin(); it != robot_state_.exclusion_checks.checks.end(); ++it)
{
RobotState::Exclusion excl = *it;
ROS_DEBUG("Name body A = %s", excl.name_body_A.c_str());
ROS_DEBUG("Name body B = %s", excl.name_body_B.c_str());
}
}
} catch(XmlRpc::XmlRpcException& ex)
{
std::cout << ex.getMessage() << std::endl;
}
}
MidemUserInteraction::MidemUserInteraction(QMainWindow *parent) :
QMainWindow(parent),
ui(new Ui::MidemUserInteraction),
quit_thread_(false),
nhp_("~"),
working_frame_("/base_link"),
gesture_detector_loader_("midem_user_interaction","hg_gesture_detector::GestureDetector")
{
ui->setupUi(this);
dynamic_reconfigure::Server<midem_user_interaction::MidemUserInteractionConfig>::CallbackType f;
f = boost::bind(&MidemUserInteraction::callbackConfig, this, _1, _2);
reconfigure_server_.setCallback(f);
arms_sub_ = nh_.subscribe("arms_msg", 1, &MidemUserInteraction::callbackArms, this);
arm_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("arm_gestures", 1);
skeletons_sub_ = nh_.subscribe("skeletons_msg", 1, &MidemUserInteraction::callbackSkeletons, this);
skeleton_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("skeleton_gestures", 1);
arms_syn_sub_.subscribe(nh_, "arms_msg", 10);
skeletons_syn_sub_.subscribe(nh_, "skeletons_msg", 10);
arm_skeleton_sync_.reset(new message_filters::Synchronizer<ArmBodyAppoxSyncPolicy>(ArmBodyAppoxSyncPolicy(10), arms_syn_sub_, skeletons_syn_sub_));
arm_skeleton_sync_->registerCallback(boost::bind(&MidemUserInteraction::callbackArmsSkelentons, this, _1, _2));
arm_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("arm_gesture_markers", 1);
skeleton_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("skeleton_gesture_markers", 1);
XmlRpc::XmlRpcValue gesture_detector;
nhp_.getParam("gesture_detector", gesture_detector);
if(gesture_detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("invalid YAML structure");
return;
}
ROS_INFO_STREAM("load " << gesture_detector.size() << " gesture detector(s)");
for(XmlRpc::XmlRpcValue::iterator it = gesture_detector.begin(); it != gesture_detector.end(); it++)
{
ROS_INFO_STREAM("detector name: " << it->first);
XmlRpc::XmlRpcValue detector;
nhp_.getParam("gesture_detector/" + it->first, detector);
if(detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
{
ROS_ERROR("invalid YAML structure");
return;
}
if(detector.begin()->first != "type")
{
ROS_ERROR("invalid YAML structure");
return;
}
ROS_INFO_STREAM(" type: " << detector.begin()->second);
try
{
gesture_detector_map_[it->first] = gesture_detector_loader_.createInstance(detector.begin()->second);
gesture_detector_map_[it->first]->setName(it->first);
if(!gesture_detector_map_[it->first]->initialize())
{
ROS_ERROR_STREAM("Cannot initialize detector " << it->first);
gesture_detector_map_.erase(it->first);
}
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}
}
}
// WriteCameraFeaturesFromRosparam()
// Read ROS parameters from this node's namespace, and see if each parameter has a similarly named & typed feature in the camera. Then set the
// camera feature to that value. For example, if the parameter camnode/Gain is set to 123.0, then we'll write 123.0 to the Gain feature
// in the camera.
//
// Note that the datatype of the parameter *must* match the datatype of the camera feature, and this can be determined by
// looking at the camera's XML file. Camera enum's are string parameters, camera bools are false/true parameters (not 0/1),
// integers are integers, doubles are doubles, etc.
//
void WriteCameraFeaturesFromRosparam(void)
{
XmlRpc::XmlRpcValue xmlrpcParams;
XmlRpc::XmlRpcValue::iterator iter;
ArvGcNode *pGcNode;
GError *error=NULL;
global.phNode->getParam (ros::this_node::getName(), xmlrpcParams);
if (xmlrpcParams.getType() == XmlRpc::XmlRpcValue::TypeStruct)
{
for (iter=xmlrpcParams.begin(); iter!=xmlrpcParams.end(); iter++)
{
std::string key = iter->first;
pGcNode = arv_device_get_feature (global.pDevice, key.c_str());
if (pGcNode && arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error))
{
// unsigned long typeValue = arv_gc_feature_node_get_value_type((ArvGcFeatureNode *)pGcNode);
// ROS_INFO("%s cameratype=%lu, rosparamtype=%d", key.c_str(), typeValue, static_cast<int>(iter->second.getType()));
// We'd like to check the value types too, but typeValue is often given as G_TYPE_INVALID, so ignore it.
switch (iter->second.getType())
{
case XmlRpc::XmlRpcValue::TypeBoolean://if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeBoolean))// && (typeValue==G_TYPE_INT64))
{
int value = (bool)iter->second;
arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value);
ROS_INFO("Read parameter (bool) %s: %d", key.c_str(), value);
}
break;
case XmlRpc::XmlRpcValue::TypeInt: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeInt))// && (typeValue==G_TYPE_INT64))
{
int value = (int)iter->second;
arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value);
ROS_INFO("Read parameter (int) %s: %d", key.c_str(), value);
}
break;
case XmlRpc::XmlRpcValue::TypeDouble: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeDouble))// && (typeValue==G_TYPE_DOUBLE))
{
double value = (double)iter->second;
arv_device_set_float_feature_value(global.pDevice, key.c_str(), value);
ROS_INFO("Read parameter (float) %s: %f", key.c_str(), value);
}
break;
case XmlRpc::XmlRpcValue::TypeString: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeString))// && (typeValue==G_TYPE_STRING))
{
std::string value = (std::string)iter->second;
arv_device_set_string_feature_value(global.pDevice, key.c_str(), value.c_str());
ROS_INFO("Read parameter (string) %s: %s", key.c_str(), value.c_str());
}
break;
case XmlRpc::XmlRpcValue::TypeInvalid:
case XmlRpc::XmlRpcValue::TypeDateTime:
case XmlRpc::XmlRpcValue::TypeBase64:
case XmlRpc::XmlRpcValue::TypeArray:
case XmlRpc::XmlRpcValue::TypeStruct:
default:
ROS_WARN("Unhandled rosparam type in WriteCameraFeaturesFromRosparam()");
}
}
}
}
} // WriteCameraFeaturesFromRosparam()