void applySetupCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
if(params.valid()) {
if(params.size() != 1) {
result = -1;
return;
}
XmlRpc::XmlRpcValue p = params[0];
if(p.size() % 2 != 0) {
result = -1;
fflush(stdout);
return;
}
for(int i=0; i<p.size(); i+= 2) {
if(0 > ctrl_xmlrpc->addModule(p[i], p[i+1])) {
result = i+1;
fflush(stdout);
return;
}
}
}
result = 0;
fflush(stdout);
return;
}
void Costmap2DROS::readFootprintFromXMLRPC( XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name )
{
// Make sure we have an array of at least 3 elements.
if( footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3 )
{
ROS_FATAL( "The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
full_param_name.c_str(), std::string( footprint_xmlrpc ).c_str() );
throw std::runtime_error( "The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
}
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
for( int i = 0; i < footprint_xmlrpc.size(); ++i )
{
// Make sure each element of the list is an array of size 2. (x and y coordinates)
XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
if( point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
point.size() != 2 )
{
ROS_FATAL( "The footprint (parameter %s) must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
full_param_name.c_str() );
throw std::runtime_error( "The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form" );
}
pt.x = getNumberFromXMLRPC( point[ 0 ], full_param_name );
pt.y = getNumberFromXMLRPC( point[ 1 ], full_param_name );
footprint.push_back( pt );
}
setUnpaddedRobotFootprint( footprint );
}
void NavSatTransform::run()
{
ros::Time::init();
double frequency = 10.0;
double delay = 0.0;
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
// Load the parameters we need
nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
nhPriv.param("yaw_offset", yawOffset_, 0.0);
nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
nhPriv.param("zero_altitude", zeroAltitude_, false);
nhPriv.param("publish_filtered_gps", publishGps_, false);
nhPriv.param("use_odometry_yaw", useOdometryYaw_, false);
nhPriv.param("wait_for_datum", useManualDatum_, false);
nhPriv.param("frequency", frequency, 10.0);
nhPriv.param("delay", delay, 0.0);
// Subscribe to the messages and services we need
ros::ServiceServer datumServ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);
if (useManualDatum_ && nhPriv.hasParam("datum"))
{
XmlRpc::XmlRpcValue datumConfig;
try
{
double datumLat;
double datumLon;
double datumYaw;
nhPriv.getParam("datum", datumConfig);
// Handle datum specification. Users should always specify a baseLinkFrameId_ in the
// datum config, but we had a release where it wasn't used, so we'll maintain compatibility.
ROS_ASSERT(datumConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(datumConfig.size() == 4 || datumConfig.size() == 5);
useManualDatum_ = true;
std::ostringstream ostr;
if (datumConfig.size() == 4)
{
ROS_WARN_STREAM("No base_link_frame specified for the datum (parameter 5).");
ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] << " " << datumConfig[3];
std::istringstream istr(ostr.str());
istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_;
}
else if (datumConfig.size() == 5)
{
ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] <<
" " << datumConfig[3] << " " << datumConfig[4];
std::istringstream istr(ostr.str());
istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_ >> baseLinkFrameId_;
}
int main(int argc, char* argv[])
{
std::vector<ros::Subscriber> subs;
ros::init(argc, argv, "Navi2");
ros::NodeHandle nh;
XmlRpc::XmlRpcValue topicList;
ros::param::get("~inputTopics", topicList);
ROS_ASSERT(topicList.size() > 0);
ROS_ASSERT(topicList.getType() == XmlRpc::XmlRpcValue::TypeArray);
for (int i = 0; i < topicList.size(); ++i)
{
ROS_ASSERT(topicList[i].getType() == XmlRpc::XmlRpcValue::TypeString);
std::string topic = static_cast<std::string>(topicList[i]);
maps.push_back(nav_msgs::OccupancyGrid());
flags.push_back(0);
subs.push_back(nh.subscribe<nav_msgs::OccupancyGrid>(topic, 1, boost::bind(mapCallback, _1, i)));
}
ros::Publisher mapPub = nh.advertise<nav_msgs::OccupancyGrid>("output", 1);
ros::Rate rate(20);
while (ros::ok())
{
rate.sleep();
ros::spinOnce();
//Everyone ready?
if ((unsigned int)std::count(flags.begin(), flags.end(), 1) == flags.size())
{
//merge
nav_msgs::OccupancyGrid map;
map.info = maps.front().info;
for (unsigned int i = 0; i < map.info.height * map.info.width; ++i)
{
map.data.push_back(-1);
for (std::vector<nav_msgs::OccupancyGrid>::iterator j = maps.begin(); j != maps.end(); ++j)
{
map.data[i] = std::max(map.data[i], j->data[i]);
}
}
mapPub.publish(map);
//Make not ready
std::fill(flags.begin(), flags.end(), 0);
}
}
}
void PoseTracker::parseParameters(const XmlRpc::XmlRpcValue &leds)
{
ROS_ASSERT(leds.getType() == XmlRpc::XmlRpcValue::TypeArray);
// if parameters is good, resize the matrix to account for number of leds used to track
// 4 = id + 3Dposition
local_points_=Eigen::MatrixXd::Zero(leds.size(),4);
Nleds_ = leds.size();
for( int i = 0; i < Nleds_; ++i)
{
XmlRpc::XmlRpcValue current_led = leds[i];
// parse ID
ROS_ASSERT(current_led.getType() == XmlRpc::XmlRpcValue::TypeStruct);
if( current_led.hasMember("id") )
{
ROS_ASSERT(current_led["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
int id = current_led["id"];
local_points_(i, 0) = (double) id;
}
else
{
ROS_ERROR("No id value for the current led. Check the yaml configuration for this object");
return;
}
// parse position
std::vector<double> position;
if( current_led.hasMember("position") )
{
for (int j = 0; j < current_led["position"].size(); ++j)
{
ROS_ASSERT(current_led["position"][j].getType() == XmlRpc::XmlRpcValue::TypeDouble);
position.push_back( current_led["position"][j] );
}
local_points_(i, 1) = position[0];
local_points_(i, 2) = position[1];
local_points_(i, 3) = position[2];
}
else
{
ROS_ERROR("No double value for the position current led. Check the yaml configuration for this object, remember to use . dots to ensure double format");
return;
}
}
ROS_INFO("Succesfully parsed all LED parameters!");
// std::cout << local_points_ << std::endl;
return;
}
bool HmValue::ListConnections( XmlRpc::XmlRpcValue& list )
{
if( _sendingChannel )
{
XmlRpcValue dict;
dict["Alias"] = _sendingChannel->GetAlias();
dict["Name"] = _sendingChannel->GetName();
dict["DisplayName"] = _sendingChannel->GetDisplayName();
if( _channel->GetDevice()->GetInterface() )
{
dict["XmlRpcInterfaceId"] = _channel->GetDevice()->GetInterface()->GetId();
dict["XmlRpcInterfaceUrl"] = _channel->GetDevice()->GetInterface()->GetUrl();
dict["XmlRpcDevice"] = _channel->GetDevice()->GetSerial();
dict["XmlRpcChannel"] = _channel->GetSerial();
dict["XmlRpcValue"] = GetId();
}
dict["Device"] = _channel->GetDevice()->GetId();
dict["DeviceDisplayName"] = _channel->GetDevice()->GetDisplayName();
dict["ChannelIndex"] = _channel->GetIndex();
dict["Terminal"] = _channel->GetTerminal();
dict["Direction"] = "Input";
list[list.size()] = dict;
}
if( _receivingConnection )
{
std::vector<IcChannel*> receivingChannels = _receivingConnection->GetReceivingChannels();
for( unsigned int i=0; i<receivingChannels.size(); i++ )
{
XmlRpcValue dict;
dict["Alias"] = receivingChannels[i]->GetAlias();
dict["Name"] = receivingChannels[i]->GetName();
dict["DisplayName"] = receivingChannels[i]->GetDisplayName();
if( _channel->GetDevice()->GetInterface() )
{
dict["XmlRpcInterfaceId"] = _channel->GetDevice()->GetInterface()->GetId();
dict["XmlRpcInterfaceUrl"] = _channel->GetDevice()->GetInterface()->GetUrl();
dict["XmlRpcDevice"] = _channel->GetDevice()->GetSerial();
dict["XmlRpcChannel"] = _channel->GetSerial();
dict["XmlRpcValue"] = GetId();
}
dict["Device"] = _channel->GetDevice()->GetId();
dict["DeviceDisplayName"] = _channel->GetDevice()->GetDisplayName();
dict["ChannelIndex"] = _channel->GetIndex();
dict["Terminal"] = _channel->GetTerminal();
dict["Direction"] = "Output";
list[list.size()] = dict;
}
}
return true;
}
bool SensorConfiguration::XmlRpcValueToEigenXd(XmlRpc::XmlRpcValue& field,
Eigen::VectorXd *target) {
target->resize(field.size());
for (int k = 0; k < field.size(); k++) {
if (field[k].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
return false;
}
(*target)[k] = static_cast<double>(field[k]);
}
return true;
}
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);
}
}
}
}
}
template <typename T> bool read_vector(ros::NodeHandle &n_, const std::string &key, std::vector<T> & res){
XmlRpc::XmlRpcValue namesXmlRpc;
if (!n_.hasParam(key))
{
return false;
}
n_.getParam(key, namesXmlRpc);
/// Resize and assign of values to the vector
res.resize(namesXmlRpc.size());
for (int i = 0; i < namesXmlRpc.size(); i++)
{
res[i] = (T)namesXmlRpc[i];
}
return true;
}
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 startWorkingCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
// if no arguments were received
if (!params.valid())
{
result = ctrl_xmlrpc->startProcessing();
}
else
{
switch(params.size())
{
// start the server in online mode with port and blocksize
case 2:
ctrl_xmlrpc->startEegServer((int) params[0], (int) params[1]);
result = 0;
break;
case 3:
ctrl_xmlrpc->startEegServer((std::string) params[0], (int) params[1], (int) params[2]);
result = 0;
break;
default:
OMG("WARNING! Server got wrong number of arguments!");
break;
}
}
fflush(stdout);
}
/*generateConfigVector stores the pinging sensors, to which pinging sensor is a listening
* sensor bound to in a cycle and which sensors are not used at all.
* In the output, the pinging sensors are represented by PINGING_SENSOR, location of
* each listening sensor holds the address of its corresponding pinging sensor
* and the sensors not used at all are represented by SENSOR_NOT_USED.
*/
std::vector <std::vector<int> > generateConfigVector(XmlRpc::XmlRpcValue config_list)
{
std::vector<std::vector<int> > config;
XmlRpc::XmlRpcValue current_cycle;
ROS_ASSERT(config_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
for(int i = 0; i < config_list.size(); i++)
{
ROS_ASSERT(config_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
std::vector<int> cycle_vector; //For holding configuration for each cycle.
cycle_vector.resize(MAX_SENSORS);
for (int j=0; j<MAX_SENSORS; j++)
cycle_vector[j]=SENSOR_NOT_USED;
//better to keep it dynamic
for (int j = 0; j<MAX_SENSORS; j++){
char str_index[3] = {'\0','\0', '\0'}; // for storing int to hex conv.
sprintf(str_index,"%d", j); // Unsure if yamlcpp parser reads hexadecimal values
if(config_list[i].hasMember(str_index))
{
cycle_vector[j]=PINGING_SENSOR; //Set -1 for pinging sensor
current_cycle = (config_list[i]).operator[](str_index);
ROS_ASSERT(current_cycle.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(current_cycle.size()>0);
//Assign address of pinging sensor to each listening sensor
for (int k = 0; k<current_cycle.size(); k++){
cycle_vector[static_cast<int>(current_cycle[k])]=((static_cast<int>(current_cycle[k])!=j)?j:PINGING_AND_LISTENING_SENSOR);
}
}
}
config.push_back(cycle_vector);
}
return config;
}
bool TestCollisionWorld::readDoubleArray(XmlRpc::XmlRpcValue& list, std::vector<double>& array)
{
if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Parameter needs to be an *array* of doubles");
return false;
}
array.clear();
for (int32_t i = 0; i < list.size(); ++i)
{
if (list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
list[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
{
ROS_ERROR("Parameter needs to be an array of *doubles*");
return false;
}
double value=0.0;
if (list[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
value = static_cast<double>(static_cast<int>(list[i]));
else value = static_cast<double>(list[i]);
array.push_back(value);
}
return true;
}
void BeaconKFNode::getCovarianceMatrix(std::string param_name, MatrixWrapper::SymmetricMatrix& m)
{
ros::NodeHandle private_nh("~");
XmlRpc::XmlRpcValue values;
private_nh.getParam(param_name, values);
if( values.getType() != XmlRpc::XmlRpcValue::TypeArray )
{
ROS_ERROR("BEACON LOCALIZER Unable to read covariance %s, not an array", param_name.c_str());
return;
}
if( values.size() < 6 )
{
ROS_ERROR("BEACON LOCALIZER Unable to read covariance %s, array too short: %d", param_name.c_str(), values.size());
return;
}
int i=0;
for (uint32_t row = 1; row <= m.rows(); ++row)
{
for (uint32_t column = row; column <= m.columns(); ++column)
{
double x;
if(i>=values.size())
{
ROS_ERROR("BEACON LOCALIZER Need at least 6 values, have %d", i);
return;
}
XmlRpc::XmlRpcValue value=values[i++];
if( value.getType() == XmlRpc::XmlRpcValue::TypeInt )
{
x=int(value);
}
else if(value.getType() == XmlRpc::XmlRpcValue::TypeDouble )
{
x=double(value);
}
else
{
std::string vstr = value;
ROS_ERROR("BEACON LOCALIZER Unable to read covariance matrix %s, value at %d is not a number: %s",
param_name.c_str(), i, vstr.c_str());
return;
}
m(row, column) = x;
}
}
}
bool PTPFollowJointTrajectoryController::init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle &root_nh, ros::NodeHandle& controller_nh)
{
nh_ = controller_nh;
// get all joint states from the hardware interface
const std::vector<std::string>& joint_names = hw->getNames();
for (unsigned i = 0; i < joint_names.size(); i++)
ROS_DEBUG_NAMED("PTPFollowJointController", "Got joint %s", joint_names[i].c_str());
XmlRpc::XmlRpcValue joints;
if (!controller_nh.getParam("joints", joints))
{
ROS_ERROR("No joints array setting");
return false;
}
if (joints.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR("Joints array setting setting type mismatched");
return false;
}
for (size_t i = 0; i < joints.size(); i++)
{
std::string joint_name;
ROS_ASSERT(joints[i].getType() == XmlRpc::XmlRpcValue::TypeString);
joint_name = static_cast<std::string>(joints[i]);
for(size_t j = 0; j < joint_names.size(); j++)
{
if(joint_name == joint_names[i])
{
ROS_DEBUG("Add joint %s to PTPFollowJointController", joint_name.c_str());
joints_.push_back(hw->getHandle(joint_name));
break;
}
}
}
q.resize(joints_.size());
qd.resize(joints_.size());
qdd.resize(joints_.size());
// Creates a dummy trajectory
boost::shared_ptr<SpecifiedTrajectory> traj_ptr(new SpecifiedTrajectory(1));
SpecifiedTrajectory &traj = *traj_ptr;
traj[0].duration = 0.0;
traj[0].splines.resize(joints_.size());
for (size_t j = 0; j < joints_.size(); ++j)
traj[0].splines[j].coef[0] = 0.0;
current_trajectory_box_.set(traj_ptr);
action_server_follow_.reset(new FJTAS(controller_nh, "ptp_follow_joint_trajectory",
boost::bind(&PTPFollowJointTrajectoryController::goalCBFollow, this, _1),
boost::bind(&PTPFollowJointTrajectoryController::cancelCBFollow, this, _1),
false));
action_server_follow_->start();
return true;
}
void
onInit()
{
n_ = getNodeHandle();
ros::NodeHandle pn = getPrivateNodeHandle();
pn.getParam("target_frame_id", target_frame_id_);
pn.getParam("table_frame_id", table_frame_id_);
pn.getParam("height_min", height_min_);
pn.getParam("height_max", height_max_);
XmlRpc::XmlRpcValue v;
pn.getParam("table_dimensions", v);
if( v.size() < 3)
{
ROS_ERROR("Hull points not set correctly, nodelet will not work");
return;
}
double x ,y, z;
x = (double)v[0];
y = (double)v[1];
z = (double)v[2];
Eigen::Vector3d p;
p << -x/2, -y/2, z;
hull_points_[0] = p;
p << -x/2, y/2, z;
hull_points_[1] = p;
p << x/2, y/2, z;
hull_points_[2] = p;
p << x/2, -y/2, z;
hull_points_[3] = p;
tf::StampedTransform trf_table;
try
{
tf_listener_.waitForTransform(target_frame_id_, table_frame_id_, ros::Time(), ros::Duration(2));
tf_listener_.lookupTransform(target_frame_id_, table_frame_id_, ros::Time(), trf_table);
}
catch (tf::TransformException& ex) {
ROS_ERROR("[transform region crop] : %s",ex.what());
return;
}
Eigen::Affine3d ad;
tf::transformTFToEigen(trf_table, ad);
for(int i = 0; i < hull_points_.size(); i++)
{
Point p;
p.getVector3fMap() = (ad*hull_points_[i]).cast<float>();
hull_->points[i] = p;
}
pc_sub_ = n_.subscribe<PointCloud>("point_cloud_in", 1, boost::bind(&TableRegionCropNodelet::topicCallback, this, _1));
pc_pub_ = n_.advertise<PointCloud>("point_cloud_out", 1);
eppd_.setInputPlanarHull(hull_);
eppd_.setHeightLimits(height_min_, height_max_);
eppd_.setViewPoint(0,0,5);
}
void ServoInterface::loadServoParams()
{
//read in servo settings
ros::NodeHandle nhPvt = getPrivateNodeHandle();
XmlRpc::XmlRpcValue v;
nhPvt.param("servos", v, v);
if(v.getType() == XmlRpc::XmlRpcValue::TypeStruct)
{
XmlRpc::XmlRpcValue servoInfo;
ServoSettings toAdd;
for(int i = 0; i < v.size(); i++)
{
servoInfo = v[boost::lexical_cast<std::string>(i)];
if(servoInfo.getType() == XmlRpc::XmlRpcValue::TypeStruct &&
servoInfo.size() == 5)
{
toAdd.center = static_cast<int>(servoInfo["center"]);
toAdd.min = static_cast<int>(servoInfo["min"]);
toAdd.max = static_cast<int>(servoInfo["max"]);
toAdd.range = toAdd.max-toAdd.min;
toAdd.port = i;
toAdd.reverse = static_cast<bool>(servoInfo["reverse"]);
m_servoSettings[servoInfo["name"]] = toAdd;
} else
{
NODELET_ERROR("ServoInterface: XmlRpc servo settings formatted incorrectly");
}
}
} else
{
NODELET_ERROR("ServoInterface: Couldn't retreive servo settings");
}
NODELET_INFO("ServoInterface: Loaded %lu servos", m_servoSettings.size());
if(m_servoSettings.find("frontBrake") != m_servoSettings.end())
{
m_brakeSetup.independentFront = true;
}
if(m_servoSettings.find("backBrake") != m_servoSettings.end())
{
m_brakeSetup.independentBack = true;
}
}
// Replacement "shutdown" XMLRPC callback
void shutdownCallback( XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result )
{
if( (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
&& (params.size() > 1) ) {
std::string reason = params[1];
ROS_WARN( "Shutdown request received. Reason: [%s]", reason.c_str() );
g_request_shutdown = 1;
}
result = ros::xmlrpc::responseInt( 1, "", 0 );
}
// Constructor
Propulsion::Propulsion(ModelPlane * parent, int ID)
{
parentObj = parent;
id = ID;
XmlRpc::XmlRpcValue list;
int i;
char paramMsg[50];
sprintf(paramMsg, "motor%i/CGOffset", id);
if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -%s- in param server!", paramMsg); ros::shutdown();}
for (i = 0; i < list.size(); ++i) {
ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
CGOffset.x = list[0];
CGOffset.y = list[1];
CGOffset.z = list[2];
sprintf(paramMsg, "motor%i/mountOrientation", id);
if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -%s- in param server!", paramMsg); ros::shutdown();}
for (i = 0; i < list.size(); ++i) {
ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
}
// !!! Order mixed because tf::Quaternion::setEuler seems to work with PRY, instead of YPR
mountOrientation.y = list[0];
mountOrientation.z = list[1];
mountOrientation.x = list[2];
theta = 0; // Initialize propeller angle
sprintf(paramMsg, "motor%i/chanMotor", id);
if(!ros::param::getCached(paramMsg, chanMotor)) {ROS_INFO("No MOTOR%i channel selected", id); chanMotor=-1;}
sprintf(paramMsg, "motor%i/chanGimbal", id);
if(!ros::param::getCached(paramMsg, chanGimbal)) {ROS_INFO("No GIMBAL%i channel selected", id); chanGimbal=-1;}
sprintf(paramMsg, "motor%i/gimbalAngle_max", id);
if(!ros::param::getCached(paramMsg, gimbalAngle_max)) {ROS_INFO("No GIMBALANGLE_MAX%i value selected", id); gimbalAngle_max=0.0;}
inputMotor = 0.0;
inputGimbal = 0.0;
sprintf(paramMsg, "motor%i/rotationDir", id);
if(!ros::param::getCached(paramMsg, rotationDir)) {ROS_INFO("No ROTATION_DIR%i value selected", id); rotationDir=1.0;}
}
bool SensorConfiguration::XmlRpcValueToEigenXd(XmlRpc::XmlRpcValue& field,
Eigen::MatrixXd *target) {
int n = std::sqrt(field.size());
if (field.size() != std::pow(n, 2)) {
return false;
}
target->resize(n, n);
for (int r = 0; r < n; r++) {
for (int c = 0; c < n; c++) {
if (field[r * n + c].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
return false;
}
(*target)(r, c) = static_cast<double>(field[r * n + c]);
}
}
return true;
}
QLearner::QLearner(ros::NodeHandle nh)
{
n_ = nh;
ros::NodeHandle n_private("~");
n_private.param("num_states", num_states_, 8);
n_private.param("num_actions", num_actions_, 3);
n_private.param("learning_rate", learning_rate_, 0.3);
n_private.param("discount_factor", discount_factor_, 0.5);
n_private.param("temp_const", temp_const_, 5.0);
n_private.param("temp_alpha", temp_alpha_, 0.85);
temp_cnt_ = 0;
temp_ = temp_const_ * pow(temp_alpha_, temp_cnt_);
size_array_ = num_actions_ * num_states_;
srand ( time(NULL) );
Init();
if (n_private.hasParam("qarray"))
{
learn_ = false;
// Get the qtable
XmlRpc::XmlRpcValue qtable;
n_private.getParam("qarray", qtable);
if (qtable.getType() != XmlRpc::XmlRpcValue::TypeArray)
ROS_ERROR("Error reading footsteps/x from config file.");
int size;
try
{
size = qtable.size();
if (size != (num_states_ * num_actions_))
{
ROS_ERROR("Size of array does not match num_states * num_actions = %d,\
exiting.", num_states_ * num_actions_);
exit(0);
}
}
catch (const XmlRpc::XmlRpcException e)
{
ROS_ERROR("No table available, exiting.");
exit(0);
}
// create qarray set
for(int i=0; i < size; i++)
{
q_array_.push_back((double)qtable[i]);
}
}
/*
parameter format is:
polygon_array: [[[0, 0, 0], [0, 0, 1], [1, 0, 0]], ...]
*/
bool StaticPolygonArrayPublisher::readPolygonArray(const std::string& param_name)
{
if (pnh_->hasParam(param_name)) {
XmlRpc::XmlRpcValue v;
pnh_->param(param_name, v, v);
if (v.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t toplevel_i = 0; toplevel_i < v.size(); toplevel_i++) { // polygons
XmlRpc::XmlRpcValue polygon_v = v[toplevel_i];
geometry_msgs::PolygonStamped polygon;
if (polygon_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
polygon_v.size() >= 3) {
for (size_t secondlevel_i = 0; secondlevel_i < polygon_v.size(); secondlevel_i++) { // each polygon, vertices
XmlRpc::XmlRpcValue vertex_v = polygon_v[secondlevel_i];
if (vertex_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
vertex_v.size() == 3 ) { // vertex_v := [x, y, z]
double x = getXMLDoubleValue(vertex_v[0]);
double y = getXMLDoubleValue(vertex_v[1]);
double z = getXMLDoubleValue(vertex_v[2]);
geometry_msgs::Point32 point;
point.x = x;
point.y = y;
point.z = z;
polygon.polygon.points.push_back(point);
}
else {
JSK_NODELET_FATAL("%s[%lu][%lu] is not array or the length is not 3",
param_name.c_str(), toplevel_i, secondlevel_i);
return false;
}
}
polygons_.polygons.push_back(polygon);
// estimate model coefficients
coefficients_.coefficients.push_back(polygonToModelCoefficients(polygon));
}
else {
JSK_NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
return false;
}
}
return true;
}
else {
JSK_NODELET_FATAL("%s is not array", param_name.c_str());
return false;
}
}
else {
JSK_NODELET_FATAL("no %s is available on parameter server", param_name.c_str());
return false;
}
return true;
}
// Class constructor
PanosContactPoints::PanosContactPoints(ModelPlane * parent) : GroundReaction(parent)
{
XmlRpc::XmlRpcValue list;
int i, j, points;
char paramMsg[50];
double temp[6];
// Read contact points number from parameter server
if(!ros::param::getCached("airframe/contactPtsNo", contactPtsNo)) {ROS_FATAL("Invalid parameters for -/airframe/contactPtsNo- in param server!"); ros::shutdown();}
// Create an appropriately sized matrix to contain contact point information
pointCoords = (double*)malloc(sizeof(double) * contactPtsNo*3); // contact points coordinates in the body frame
materialIndex = (double*)malloc(sizeof(double) * contactPtsNo*1); // contact points material type index
springIndex = (double*)malloc(sizeof(double) * contactPtsNo*2); // contact points spring characteristics
len=0.2;
// Set coefficient of friction for each material
frictForw[0] = 0.7; frictSide[0] = 0.7;
frictForw[1] = 0.4; frictSide[1] = 0.4;
frictForw[2] = 0.1; frictSide[2] = 1.0;
frictForw[3] = 0.4; frictSide[3] = 0.4; //To update composite to ground firction coefficients!
// Read contact points location and material from parameter server
for (j = 0; j<contactPtsNo; j++) { //Distribute the data
sprintf(paramMsg, "airframe/contactPoint%i", j+1);
if(!ros::param::getCached(paramMsg, list)) {ROS_FATAL("Invalid parameters for -/airframe/contactPoint- in param server!"); ros::shutdown();}
for (i = 0; i < list.size(); ++i) {
ROS_ASSERT(list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
temp[i]=list[i];
}
pointCoords[j] = temp[0]; // Save body frame contact point coordinates
pointCoords[j + contactPtsNo] = temp[1];
pointCoords[j + contactPtsNo*2] = temp[2];
materialIndex[j] = temp[3]; // A separate contact point material index array
springIndex[j] = temp[4]; // And the spring constants
springIndex[j + contactPtsNo] = temp[5];
}
// Create and initialize spring contraction container
spp = (double*)malloc(sizeof(double) * contactPtsNo);
memset(spp, 0, sizeof(spp));
// Create and initialize previous spring contraction container
sppprev = (double*)malloc(sizeof(double) * contactPtsNo);
memset(sppprev, 0, sizeof(sppprev));
contact = false;
// Create other matrices needed for calculations
cpi_up = (double*)malloc(sizeof(double) * contactPtsNo*3); // upper spring end matrix
cpi_down = (double*)malloc(sizeof(double) * contactPtsNo*3); // lower spring end matrix
spd = (double*)malloc(sizeof(double) * contactPtsNo); // spring contraction speed
}
void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
int num_params = 0;
if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
num_params = params.size();
if (num_params > 1)
{
std::string reason = params[1];
ROS_WARN("Shutdown request received. Reason: [%s]", reason.c_str());
::gShutdownRequest = 1; // Set flag
}
result = ros::xmlrpc::responseInt(1, "", 0);
}
/*!
* \brief Routine for publishing joint_states.
*
* Gets current positions and velocities from the hardware and publishes them as joint_states.
*/
void publishJointState()
{
if (isInitialized_ == true)
{
// create joint_state message
int DOF = ModIds_param_.size();
std::vector<double> ActualPos;
std::vector<double> ActualVel;
ActualPos.resize(DOF);
ActualVel.resize(DOF);
PCube_->getConfig(ActualPos);
PCube_->getJointVelocities(ActualVel);
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name = JointNames_;
for (int i = 0; i<DOF; i++ )
{
msg.position[i] = ActualPos[i];
msg.velocity[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
// publish message
topicPub_JointState_.publish(msg);
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header.stamp = ros::Time::now();
controllermsg.joint_names.resize(DOF);
controllermsg.actual.positions.resize(DOF);
controllermsg.actual.velocities.resize(DOF);
controllermsg.joint_names = JointNames_;
for (int i = 0; i<DOF; i++ )
{
controllermsg.actual.positions[i] = ActualPos[i];
controllermsg.actual.velocities[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
topicPub_ControllerState_.publish(controllermsg);
}
}
std::vector<std::string> MUX::readStringArray(std::string param_name,
ros::NodeHandle& handle)
{
// read string array
std::vector<std::string> strings;
XmlRpc::XmlRpcValue v;
if (handle.hasParam(param_name)) {
handle.param(param_name, v, v);
for (size_t i = 0; i < v.size(); i++) {
strings.push_back(v[i]);
}
}
return strings;
}
/*!
* \brief Executes the callback from the actionlib.
*
* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
* \param goal JointTrajectoryGoal
*/
void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
if (!isInitialized_)
{
ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
as_.setAborted();
return;
}
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
finished_ = false;
// stoping arm to prepare for new trajectory
std::vector<double> VelZero;
VelZero.resize(ModIds_param_.size());
PCube_->MoveVel(VelZero);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
usleep(500000); // needed sleep until powercubes starts to change status from idle to moving
while(finished_ == false)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeed
//result_.result.data = "executing trajectory";
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
inline bool parameterToColor(const ros::NodeHandle& node, const
std::string& key, float* color) {
XmlRpc::XmlRpcValue value;
node.getParam(key, value);
if (value.getType() == XmlRpc::XmlRpcValue::TypeArray) {
if (value.size() == 3) {
color[0] = static_cast<double>(value[0]);
color[1] = static_cast<double>(value[1]);
color[2] = static_cast<double>(value[2]);
}
else {
ROS_WARN("Invalid array size for color parameter %s: %d", key.c_str(),
(unsigned int)value.size());
return false;
}
}
else {
ROS_WARN("Invalid type for color parameter %s: expecting array",
key.c_str(), (unsigned int)value.size());
return false;
}
return true;
}
bool getNodes(V_string& nodes)
{
XmlRpc::XmlRpcValue args, result, payload;
args[0] = this_node::getName();
if (!execute("getSystemState", args, result, payload, true))
{
return false;
}
S_string node_set;
for (int i = 0; i < payload.size(); ++i)
{
for (int j = 0; j < payload[i].size(); ++j)
{
XmlRpc::XmlRpcValue val = payload[i][j][1];
for (int k = 0; k < val.size(); ++k)
{
std::string name = payload[i][j][1][k];
node_set.insert(name);
}
}
}
nodes.insert(nodes.end(), node_set.begin(), node_set.end());
return true;
}
void FootstepMarker::readPoseParam(ros::NodeHandle& pnh, const std::string param,
tf::Transform& offset) {
XmlRpc::XmlRpcValue v;
geometry_msgs::Pose pose;
if (pnh.hasParam(param)) {
pnh.param(param, v, v);
// check if v is 7 length Array
if (v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
v.size() == 7) {
// safe parameter access by getXMLDoubleValue
pose.position.x = getXMLDoubleValue(v[0]);
pose.position.y = getXMLDoubleValue(v[1]);
pose.position.z = getXMLDoubleValue(v[2]);
pose.orientation.x = getXMLDoubleValue(v[3]);
pose.orientation.y = getXMLDoubleValue(v[4]);
pose.orientation.z = getXMLDoubleValue(v[5]);
pose.orientation.w = getXMLDoubleValue(v[6]);
// converst the message as following: msg -> eigen -> tf
//void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
Eigen::Affine3d e;
tf::poseMsgToEigen(pose, e); // msg -> eigen
tf::transformEigenToTF(e, offset); // eigen -> tf
}
else {
ROS_ERROR_STREAM(param << " is malformed, which should be 7 length array");
}
}
else {
ROS_WARN_STREAM("there is no parameter on " << param);
}
}
