void getParams(const ros::NodeHandle &nh)
{
//avg accel update period (number of msgs until next localization udpate)
if (nh.getParam("update_period", UPDATE_PERIOD))
{
ROS_INFO("Set %s/update_period to %d",nh.getNamespace().c_str(), UPDATE_PERIOD);
}
else
{
if(nh.hasParam("update_period"))
ROS_WARN("%s/update_period must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
else
ROS_WARN("No value set for %s/update_period. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
UPDATE_PERIOD = DEFAULT_LOC_NODE_UPDATE_PERIOD;
}
//frequency this node publishes a new topic
if (nh.getParam("publish_freq", PUBLISH_FREQ))
{
ROS_INFO("Set %s/publish_freq to %d",nh.getNamespace().c_str(), PUBLISH_FREQ);
}
else
{
if(nh.hasParam("publish_freq"))
ROS_WARN("%s/publish_freq must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
else
ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
PUBLISH_FREQ = DEFAULT_LOC_NODE_PUBLISH_FREQ;
}
//number of states from coax_filter this node will buffer before it begins to drop them
if (nh.getParam("fstate_msg_buffer", FSTATE_MSG_BUFFER))
{
ROS_INFO("Set %s/fstate_msg_buffer to %d",nh.getNamespace().c_str(), FSTATE_MSG_BUFFER);
}
else
{
if(nh.hasParam("fstate_msg_buffer"))
ROS_WARN("%s/fstate_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
else
ROS_WARN("No value set for %s/fstate_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
FSTATE_MSG_BUFFER = DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER;
}
//number of messages this node will queue for publishing before droping old data
if (nh.getParam("msg_queue", MSG_QUEUE))
{
ROS_INFO("Set %s/msg_queue to %d",nh.getNamespace().c_str(), MSG_QUEUE);
}
else
{
if(nh.hasParam("msg_queue"))
ROS_WARN("%s/msg_queue must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
else
ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
MSG_QUEUE = DEFAULT_LOC_NODE_MSG_QUEUE;
}
}
int PlatformCtrlNode::init()
{
std::string kinType;
n.param<bool>("sendTransform",sendTransform,false);
if(sendTransform)
{
ROS_INFO("platform ctrl node: sending transformation");
} else {
ROS_INFO("platform ctrl node: sending no transformation");
}
n.getParam("kinematics", kinType);
if (kinType == "DiffDrive4W")
{
double wheelDiameter;
double axisLength;
DiffDrive4WKinematics* diffKin = new DiffDrive4WKinematics;
kin = diffKin;
if(n.hasParam("wheelDiameter"))
{
n.getParam("wheelDiameter",wheelDiameter);
diffKin->setWheelDiameter(wheelDiameter);
ROS_INFO("got wheeldieameter from config file");
}
else diffKin->setWheelDiameter(0.3);
if(n.hasParam("robotWidth"))
{
n.getParam("robotWidth",axisLength);
diffKin->setAxisLength(axisLength);
ROS_INFO("got axis from config file");
}
else diffKin->setAxisLength(1);
}
else
{
ROS_ERROR("neo_PlatformCtrl-Error: unknown kinematic model");
}
if(kin == NULL) return 1;
p.xAbs = 0; p.yAbs = 0; p.phiAbs = 0;
topicPub_Odometry = n.advertise<nav_msgs::Odometry>("/odom",1);
topicSub_DriveState = n.subscribe("/joint_states",1,&PlatformCtrlNode::receiveOdo, this);
topicPub_DriveCommands = n.advertise<trajectory_msgs::JointTrajectory>("/cmd_joint_traj",1);
topicSub_ComVel = n.subscribe("/cmd_vel",1,&PlatformCtrlNode::receiveCmd, this);
return 0;
}
void loadVehParam(const std::string& veh_type)
{
if (not nh_.hasParam(veh_type)){
// Does not have such vehicle type defined
return;
}
rvo_ros::AgentParam param;
double neighborDist; nh_.param<double>(veh_type + "/neighborDist", neighborDist, 10);
int maxNeighbors; nh_.param<int>(veh_type + "/maxNeighbors", maxNeighbors, 25);
double timeHorizon; nh_.param<double>(veh_type + "/timeHorizon", timeHorizon, 1.5);
double timeHorizonObst; nh_.param<double>(veh_type + "/timeHorizonObst", timeHorizonObst, 1.2);
double radius; nh_.param<double>(veh_type + "/radius", radius, 0.4);
double maxSpeed; nh_.param<double>(veh_type + "/maxSpeed", maxSpeed, 1.2);
double maxAcc; nh_.param<double>(veh_type + "/maxAcc", maxAcc, 10.0);
double lambdaValue; nh_.param<double>(veh_type + "/lambdaValue", lambdaValue, 0.6);
param.agentType=rvo_ros::AgentParam::NORMAL;
param.neighborDist = neighborDist;
param.maxNeighbors = maxNeighbors;
param.timeHorizon = timeHorizon;
param.timeHorizonObst = timeHorizonObst;
param.radius = radius;
param.maxSpeed = maxSpeed;
param.maxAcc = maxAcc;
param.lambdaValue = lambdaValue;
rvo_param_map_[veh_type] = param;
// ROS_INFO_STREAM("[loadVehParam]:" << veh_type << ":" << param);
}
int NodeClass::init()
{
if (n.hasParam("ComPort"))
{
n.getParam("ComPort", sComPort);
ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
}
else
{
sComPort ="/dev/ttyUSB0";
ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
}
n.param("message_timeout", relayboard_timeout_, 2.0);
n.param("protocol_version", protocol_version_, 1);
m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());
m_SerRelayBoard->init();
// Init member variable for EM State
EM_stop_status_ = ST_EM_ACTIVE;
duration_for_EM_free_ = ros::Duration(1);
return 0;
}
bool setup(ros::NodeHandle nh){
if (!nh.hasParam("/has_parameters")){
return false;
}
nh.getParam("/rot_spread", rot_spread);
nh.getParam("/vel_spread", vel_spread);
nh.getParam("/updates_before_resample", updates_before_resample);
nh.getParam("/times_to_resample", times_to_resample);
nh.getParam("/prob_sigma", prob_sigma);
nh.getParam("/maze_xmax", maze_xmax);
nh.getParam("/maze_ymax", maze_ymax);
std::vector<double> ir_positions_temp;
for(int i=0;i<NUM_OBSERVATIONS;i++){
std::string temp = "/ir_positions_";
temp += boost::lexical_cast<std::string>(i+1);
nh.getParam(temp, ir_positions_temp);
sensor_positions[i][0] = ir_positions_temp[0];
sensor_positions[i][1] = ir_positions_temp[1];
sensor_positions[i][2] = ir_positions_temp[2];
}
bool known_pos;
nh.getParam("/known_pos", known_pos);
if(known_pos){
nh.getParam("/start_x", position.x);
nh.getParam("/start_y", position.y);
nh.getParam("/start_theta", position.theta);
init_known_pos(position.x, position.y, position.theta);
}
else{
void init_un_known_pos(0.0,0.0,maze_xmax,maze_ymax);
}
}
void CollisionPluginLoader::setupScene(
ros::NodeHandle& nh,
const planning_scene::PlanningScenePtr& scene)
{
if (!scene)
return;
std::string param_name;
std::string collision_detector_name;
if (nh.searchParam("collision_detector", param_name))
{
nh.getParam(param_name, collision_detector_name);
}
else if (nh.hasParam("/move_group/collision_detector"))
{
// Check for existence in move_group namespace
// mainly for rviz plugins to get same collision detector.
nh.getParam("/move_group/collision_detector", collision_detector_name);
}
else
{
return;
}
if (collision_detector_name == "")
{
// This is not a valid name for a collision detector plugin
return;
}
activate(collision_detector_name, scene, true);
ROS_INFO_STREAM("Using collision detector:" << scene->getActiveCollisionDetectorName().c_str());
}
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);
}
}
}
}
}
/****************************************************
* @brief : Default constructor
* @param : ros node handler
****************************************************/
RobotObserver(ros::NodeHandle& node)
{
node_=node;
// Getting robot's id from ros param
if(node_.hasParam("my_robot_id"))
{
node_.getParam("my_robot_id",my_id_);
} else {
my_id_="PR2_ROBOT";
}
waiting_timer_ = node_.createTimer(ros::Duration(1.0), &RobotObserver::timerCallback, this, true);
timer_on_=false;
enable_event_=true;
task_started_=false;
// Advertise subscribers
fact_list_sub_ = node_.subscribe("/agent_monitor/factList", 1, &RobotObserver::factListCallback, this);
fact_area_list_sub_ = node_.subscribe("/area_manager/factList", 1, &RobotObserver::factListAreaCallback, this);
human_list_sub_ = node_.subscribe("/pdg/humanList", 1, &RobotObserver::humanListCallback, this);
object_list_sub_ = node_.subscribe("/pdg/objectList", 1, &RobotObserver::objectListCallback, this);
current_action_list_sub_ = node_.subscribe("/human_monitor/current_humans_action", 1, &RobotObserver::CurrentActionListCallback, this);
next_action_list_sub_ = node_.subscribe("/plan_executor/next_humans_actions", 1, &RobotObserver::NextActionListCallback, this);
// Advertise publishers
attention_vizu_pub_ = node_.advertise<geometry_msgs::PointStamped>("head_manager/attention_vizualisation", 5);
init_action_client_ = new InitActionClient_t("pr2motion/Init", true);
// Initialize action client for the action interface to the head controller
head_action_client_ = new HeadActionClient_t("pr2motion/Head_Move_Target", true);
// Connection to the pr2motion client
connect_port_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/connect_port");
head_stop_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/Head_Stop");
ROS_INFO("[robot_observer] Waiting for pr2motion action server to start.");
init_action_client_->waitForServer(); //will wait for infinite time
head_action_client_->waitForServer(); //will wait for infinite time
ROS_INFO("[robot_observer] pr2motion action server started.");
pr2motion::InitGoal goal_init;
init_action_client_->sendGoal(goal_init);
pr2motion::connect_port srv;
srv.request.local = "joint_state";
srv.request.remote = "joint_states";
if (!connect_port_srv_.call(srv)){
ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
}
srv.request.local = "head_controller_state";
srv.request.remote = "/head_traj_controller/state";
if (!connect_port_srv_.call(srv)){
ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
}
pr2motion::Z_Head_SetMinDuration srv_MinDuration;
srv_MinDuration.request.head_min_duration=0.6;
if(!ros::service::call("/pr2motion/Z_Head_SetMinDuration",srv_MinDuration))
ROS_ERROR("[robot_observer] Failed to call service /pr2motion/Z_Head_SetMinDuration");
state_machine_ = new ObserverStateMachine(boost::cref(this));
state_machine_->start();
ROS_INFO("[robot_observer] Starting state machine, node ready !");
}
void getParams(const ros::NodeHandle &nh)
{
//frequency this node publishes a new topic
if(nh.getParam("publish_freq", PUBLISH_FREQ))
{
ROS_INFO("Set %s/publish_freq to %d",nh.getNamespace().c_str(), PUBLISH_FREQ);
}
else
{
if(nh.hasParam("publish_freq"))
ROS_WARN("%s/publish_freq must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ);
else
ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ);
PUBLISH_FREQ = DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ;
}
//number of blob sequences from blob_filter this node will buffer before it begins to drop them
if (nh.getParam("sequence_poses_msg_buffer", SEQ_POSES_MSG_BUFFER))
{
ROS_INFO("Set %s/sequence_poses_msg_buffer to %d",nh.getNamespace().c_str(), SEQ_POSES_MSG_BUFFER);
}
else
{
if(nh.hasParam("sequences_msg_buffer"))
ROS_WARN("%s/sequences_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER);
else
ROS_WARN("No value set for %s/sequences_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER);
SEQ_POSES_MSG_BUFFER = DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER;
}
//number of messages this node will queue for publishing before it drops data
if (nh.getParam("msg_queue", MSG_QUEUE))
{
ROS_INFO("Set %s/msg_queue to %d",nh.getNamespace().c_str(), MSG_QUEUE);
}
else
{
if(nh.hasParam("msg_queue"))
ROS_WARN("%s/msg_queue must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE);
else
ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE);
MSG_QUEUE = DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE;
}
}
// Set Opencv Algorithm parameters from ROS parameter server
void setParameters( ros::NodeHandle& nodeHandle, cv::Ptr<cv::Algorithm>&& algorithm, const std::string& base_name )
{
std::vector<cv::String> parameters;
algorithm->getParams( parameters );
for ( const auto& param : parameters )
{
if ( nodeHandle.hasParam(base_name + "/" + param) )
{
int param_type = algorithm->paramType( param );
switch ( param_type )
{
case cv::Param::INT:
{
int val;
nodeHandle.getParam(base_name + "/" + param, val);
algorithm->set(param, val);
std::cout << " " << param << ": " << val << std::endl;
break;
}
case cv::Param::BOOLEAN:
{
bool val;
nodeHandle.getParam(base_name + "/" + param, val);
algorithm->set(param, val);
std::cout << " " << param << ": " << val << std::endl;
break;
}
case cv::Param::REAL:
{
double val;
nodeHandle.getParam(base_name + "/" + param, val);
algorithm->set(param, val);
std::cout << " " << param << ": " << val << std::endl;
break;
}
case cv::Param::STRING:
{
std::string val;
nodeHandle.getParam(base_name + "/" + param, val);
algorithm->set(param, val);
std::cout << " " << param << ": " << val << std::endl;
break;
}
default:
ROS_ERROR_STREAM("unknown/unsupported parameter type for ");
break;
}
}
}
}
RosTopicParser::RosTopicParser(ros::NodeHandle nh, std::string topic_name){
_topic_name = ""; _queue_size = 1;
if (nh.hasParam(topic_name)){
nh.getParam("/" + topic_name + "/" + NAME_KEY, _topic_name);
nh.getParam("/" + topic_name + "/" + QUEUE_SIZE_KEY, _queue_size);
ROS_INFO("Topic %s, queue size %i", _topic_name.c_str(), _queue_size);
}else{
ROS_WARN("Topic not found.");
}
}
T loadParam( std::string name, ros::NodeHandle &nh){
T param;
if (nh.hasParam( name )){
nh.getParam( name, param);
return param;
} else {
std::cout << "Param " << name << " does not exist." << std::endl;
exit(0);
}
}
static std::string get_cubin_path(const ros::NodeHandle& n, const char *default_path)
{
std::string path;
if (n.hasParam("/car_detector/cubin")) {
n.getParam("/car_detector/cubin", path);
} else {
path = std::string(default_path);
}
return path;
}
//! Initialize plugin - called in server constructor
void srs_env_model::CPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
// PERROR( "Initializing PointCloudPlugin" );
// Read parameters
// Point cloud publishing topic name
node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME );
// Point cloud subscribing topic name
node_handle.param("pointcloud_subscriber", m_pcSubscriberName, SUBSCRIBER_POINT_CLOUD_NAME);
// Point cloud limits
node_handle.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ);
node_handle.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ);
// Contains input pointcloud RGB?
if( node_handle.hasParam("input_has_rgb") )
{
node_handle.param("input_has_rgb", m_bUseRGB, m_bUseRGB );
m_bRGB_byParameter = true;
}
// Create publisher
m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 100, m_latchedTopics);
// If should subscribe, create message filter and connect to the topic
if( m_bSubscribe )
{
//PERROR("Subscribing to: " << m_pcSubscriberName );
// Create subscriber
m_pcSubscriber = new message_filters::Subscriber<tIncommingPointCloud>(node_handle, m_pcSubscriberName, 1);
if (!m_pcSubscriber)
{
ROS_ERROR("Not subscribed...");
PERROR( "Not subscirbed to point clouds subscriber...");
}
// Create message filter
m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_pcFrameId, 1);
m_tfPointCloudSub->registerCallback(boost::bind( &CPointCloudPlugin::insertCloudCallback, this, _1));
//std::cerr << "SUBSCRIBER NAME: " << m_pcSubscriberName << ", FRAMEID: " << m_pcFrameId << std::endl;
}
// Clear old pointcloud data
m_data->clear();
// PERROR( "PointCloudPlugin initialized..." );
}
// Constructor
NodeClass()
{
// initialization of variables
m_bisInitialized = false;
/// Parameters are set within the launch file
// Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl.
if (n.hasParam("IniDirectory"))
{
n.getParam("IniDirectory", sIniDirectory);
ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
}
else
{
sIniDirectory = "Platform/IniFiles/";
ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
}
n.param<bool>("PublishEffort", m_bPubEffort, false);
if(m_bPubEffort) ROS_INFO("You have choosen to publish effort of motors, that charges capacity of CAN");
IniFile iniFile;
iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
// get max Joint-Velocities (in rad/s) for Steer- and Drive-Joint
iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumMotors, true);
iniFile.GetKeyInt("Config", "NumberOfWheels", &m_iNumDrives, true);
if(m_iNumMotors < 2 || m_iNumMotors > 8) {
m_iNumMotors = 8;
m_iNumDrives = 4;
}
m_CanCtrlPltf = new CanCtrlPltfCOb3(sIniDirectory);
// implementation of topics
// published topics
topicPub_JointState = n.advertise<sensor_msgs::JointState>("joint_states", 1); //EXP: not anymore /joint_states but local ns
topicPub_Diagnostic = n.advertise<diagnostic_msgs::DiagnosticStatus>("diagnostic", 1);
// subscribed topics
topicSub_JointStateCmd = n.subscribe("joint_command", 1, &NodeClass::topicCallback_JointStateCmd, this);
// implementation of service servers
srvServer_Init = n.advertiseService("init", &NodeClass::srvCallback_Init, this);
srvServer_ElmoRecorderConfig = n.advertiseService("ElmoRecorderConfig", &NodeClass::srvCallback_ElmoRecorderConfig, this);
srvServer_ElmoRecorderReadout = n.advertiseService("ElmoRecorderReadout", &NodeClass::srvCallback_ElmoRecorderReadout, this);
m_bReadoutElmo = false;
srvServer_Recover = n.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
srvServer_Shutdown = n.advertiseService("shutdown", &NodeClass::srvCallback_Shutdown, this);
//srvServer_isPltfError = n.advertiseService("isPltfError", &NodeClass::srvCallback_isPltfError, this); --> Publish this along with JointStates
srvServer_GetJointState = n.advertiseService("GetJointState", &NodeClass::srvCallback_GetJointState, this);
}
void CSrf10Controller::createDistanceSensor(std::string paramName,std::string sensorName,std::string topic, ros::NodeHandle& nh){
if(!nh.hasParam(paramName+"/address"))
{
ROS_WARN_STREAM("You need to set the address atribute for the sensor " << sensorName);
return;
}
if(!nh.hasParam(paramName+"/type"))
{
ROS_WARN_STREAM("You need to set the type of the sensor " << sensorName);
return;
}
int address;
std::string type;
std::string frame_id;
std::string sensor_topic;
double max_alert_distance;
double min_alert_distance;
nh.getParam(paramName+"/address", address);
nh.getParam(paramName+"/type", type);
nh.param(paramName+"/frame_id", frame_id, std::string(""));
nh.param(paramName+"/topic", sensor_topic, topic+"/"+sensorName);
if(!nh.hasParam(paramName+"/publish_if_obstacle"))
nh.setParam(paramName+"/publish_if_obstacle", false);
if (type.compare("srf10")==0)
{
srf10Sensors_[(uint8_t)address]=new CDistanceSensor(sensorName, (uint8_t)address, sensor_topic, nh, type, frame_id);
srf10SensorsUpdateGroup_[(uint8_t)address]=1;
}
else if (type.compare("gp2d12")==0 || type.compare("gp2d120")==0 || type.compare("GP2Y0A21YK")==0)
{
adcSensors_[(uint8_t)address]=new CDistanceSensor(sensorName, (uint8_t)address, sensor_topic, nh, type, frame_id);
adcSensorsAddresses_.push_back((uint8_t)address);
}
ROS_INFO_STREAM("Sensor " << sensorName << " of type " << type << " initialized");
}
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;
}
// Constructor
NodeClass()
{
// initialization of variables
is_initialized_bool_ = false;
iwatchdog_ = 0;
last_time_ = ros::Time::now();
x_rob_m_ = 0.0;
y_rob_m_ = 0.0;
theta_rob_rad_ = 0.0;
// set status of drive chain to WARN by default
drive_chain_diagnostic_ = diagnostic_status_lookup_.OK; //WARN; <- THATS FOR DEBUGGING ONLY!
// Parameters are set within the launch file
// Read number of drives from iniFile and pass IniDirectory to CobPlatfCtrl.
if (n.hasParam("IniDirectory"))
{
n.getParam("IniDirectory", sIniDirectory);
ROS_INFO("IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
}
else
{
sIniDirectory = "Platform/IniFiles/";
ROS_WARN("IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
}
IniFile iniFile;
iniFile.SetFileName(sIniDirectory + "Platform.ini", "PltfHardwareCoB3.h");
iniFile.GetKeyInt("Config", "NumberOfMotors", &m_iNumJoints, true);
ucar_ctrl_ = new UndercarriageCtrlGeom(sIniDirectory);
// implementation of topics
// published topics
topic_pub_joint_state_cmd_ = n.advertise<sensor_msgs::JointState>("joint_command", 1);
topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("odometry", 50);
// subscribed topics
topic_sub_CMD_pltf_twist_ = n.subscribe("command", 1, &NodeClass::topicCallbackTwistCmd, this);
topic_sub_EM_stop_state_ = n.subscribe("/emergency_stop_state", 1, &NodeClass::topicCallbackEMStop, this);
topic_sub_drive_diagnostic_ = n.subscribe("diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);
//<diagnostic_msgs::DiagnosticStatus>("Diagnostic", 1);
// implementation of service servers
//--
// implementation of service clients
srv_client_get_joint_state_ = n.serviceClient<cob_srvs::GetJointState>("GetJointState");
}
std::vector<std::string> ThrusterAllocator::initControl(ros::NodeHandle &nh, double map_threshold)
{
std::vector<std::string> controlled_axes;
const std::vector<std::string> axes{"x", "y", "z", "roll", "pitch", "yaw"};
for(size_t axis = 0; axis < 6; axis++)
{
if(max_wrench[axis] > 1e-6)
{
char param[FILENAME_MAX];
sprintf(param, "controllers/%s", axes[axis].c_str());
if(nh.hasParam(param))
{
controlled_axes.push_back(axes[axis]);
// push to position PID
sprintf(param, "controllers/%s/position/i_clamp", axes[axis].c_str());
nh.setParam(param, max_wrench[axis]);
// push to velocity PID
sprintf(param, "controllers/%s/velocity/i_clamp", axes[axis].c_str());
nh.setParam(param, max_wrench[axis]);
}
}
}
// threshold on map before pseudo-inverse
for(int r = 0; r < 6; ++r)
{
for(int c = 0; c < map.cols(); ++c)
{
if(std::abs(map(r,c)) < map_threshold)
map(r,c) = 0;
}
}
inverse_map.resize(map.cols(), map.rows());
Eigen::JacobiSVD<Eigen::MatrixXd> svd_M(map, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::VectorXd dummy_in(map.rows());
Eigen::VectorXd dummy_out(map.cols());
unsigned int i,j;
for(i=0;i<map.rows();++i)
{
dummy_in.setZero();
dummy_in(i) = 1;
dummy_out = svd_M.solve(dummy_in);
for(j = 0; j<map.cols();++j)
inverse_map(j,i) = dummy_out(j);
}
return controlled_axes;
}
double getFrequency()
{
double frequency;
if (n_.hasParam("frequency"))
{
n_.getParam("frequency", frequency);
ROS_INFO("Setting controller frequency to %f HZ", frequency);
}
else
{
frequency = 100; //Hz
ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);
}
return frequency;
}
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;
}
FiducialPoseEstimator( ros::NodeHandle& nh, ros::NodeHandle& ph )
: _fiducialManager( _lookupInterface ),
_extrinsicsInterface( nh, ph )
{
GetParam<std::string>( ph, "reference_frame", _refFrame, "" );
GetParam<std::string>( ph, "body_frame", _robotFrame, "base_link");
bool combineDetect;
GetParam( ph, "combine_detections", combineDetect, true);
unsigned int inBuffSize, outBuffSize;
GetParam( ph, "input_buffer_size", inBuffSize, (unsigned int) 20 );
GetParam( ph, "output_buffer_size", outBuffSize, (unsigned int) 20 );
_enableVis = ph.hasParam( "visualization" );
if( _enableVis )
{
ros::NodeHandle ch( ph.resolveName( "visualization/camera" ) );
ros::NodeHandle fh( ph.resolveName( "visualization/fiducial" ) );
_camVis.ReadParams( ch );
_fidVis.ReadParams( fh );
std::string refFrame;
GetParamRequired( ph, "visualization/reference_frame", refFrame );
_camVis.SetFrameID( refFrame );
_fidVis.SetFrameID( refFrame );
_visPub = nh.advertise<MarkerMsg>( "markers", 10 );
}
if( combineDetect )
{
_detSub = nh.subscribe( "detections",
inBuffSize,
&FiducialPoseEstimator::DetectionsCallbackCombined,
this );
}
else
{
_detSub = nh.subscribe( "detections",
inBuffSize,
&FiducialPoseEstimator::DetectionsCallbackIndependent,
this );
}
_posePub = ph.advertise<geometry_msgs::TransformStamped>( "relative_pose",
outBuffSize );
}
MoveMaster() : ac("object_recognition", true) {
n = ros::NodeHandle();
ROS_INFO("Waiting for action server to start...");
ROS_INFO("Action server started, sending goal.");
img_position_sub = n.subscribe("/object_detection/object_position", 1, &MoveMaster::imageDetectedCallback, this);
check_map_client = n.serviceClient<robot_msgs::checkObjectInMap>("/object_in_map");
maze_follower_client = n.serviceClient<robot_msgs::useMazeFollower>("/use_maze_follower");
path_follower_client = n.serviceClient<robot_msgs::useMazeFollower>("/use_path_follower");
ac.waitForServer();
timer = n.createTimer(ros::Duration(2*60), &MoveMaster::timerCallback, this, true);
srv_maze.request.go = true;
(maze_follower_client.call(srv_maze));
PHASE=1;
if(n.hasParam("PHASE")){
n.getParam("PHASE",PHASE);
}
start_time=ros::Time::now();
}
/**
format is
successors:
- x: 0
y: 0
theta: 0
- x: 0
y: 0
theta: 0
...
*/
bool FootstepPlanner::readSuccessors(ros::NodeHandle& nh)
{
successors_.clear();
if (!nh.hasParam("successors")) {
JSK_ROS_FATAL("no successors are specified");
return false;
}
XmlRpc::XmlRpcValue successors_xml;
nh.param("successors", successors_xml, successors_xml);
if (successors_xml.getType() != XmlRpc::XmlRpcValue::TypeArray)
{
JSK_ROS_FATAL("successors should be an array");
return false;
}
for (size_t i_successors = 0; i_successors < successors_xml.size(); i_successors++) {
XmlRpc::XmlRpcValue successor_xml;
successor_xml = successors_xml[i_successors];
if (successor_xml.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
JSK_ROS_FATAL("element of successors should be an dictionary");
return false;
}
double x = 0;
double y = 0;
double theta = 0;
if (successor_xml.hasMember("x")) {
x = jsk_topic_tools::getXMLDoubleValue(successor_xml["x"]);
}
if (successor_xml.hasMember("y")) {
y = jsk_topic_tools::getXMLDoubleValue(successor_xml["y"]);
}
if (successor_xml.hasMember("theta")) {
theta = jsk_topic_tools::getXMLDoubleValue(successor_xml["theta"]);
}
Eigen::Affine3f successor = affineFromXYYaw(x, y, theta);
successors_.push_back(successor);
}
JSK_ROS_INFO("%lu successors are defined", successors_.size());
return true;
}
// Constructor
NodeClass()
{
// create a handle for this node, initialize node
nh = ros::NodeHandle("~");
if(!nh.hasParam("port")) ROS_WARN("Used default parameter for port");
nh.param("port", port, std::string("/dev/ttyUSB0"));
if(!nh.hasParam("baud")) ROS_WARN("Used default parameter for baud");
nh.param("baud", baud, 500000);
if(!nh.hasParam("scan_id")) ROS_WARN("Used default parameter for scan_id");
nh.param("scan_id", scan_id, 7);
if(!nh.hasParam("inverted")) ROS_WARN("Used default parameter for inverted");
nh.param("inverted", inverted, false);
if(!nh.hasParam("frame_id")) ROS_WARN("Used default parameter for frame_id");
nh.param("frame_id", frame_id, std::string("/base_laser_link"));
if(!nh.hasParam("scan_duration")) ROS_WARN("Used default parameter for scan_duration");
nh.param("scan_duration", scan_duration, 0.025); //no info about that in SICK-docu, but 0.025 is believable and looks good in rviz
if(!nh.hasParam("scan_cycle_time")) ROS_WARN("Used default parameter for scan_cycle_time");
nh.param("scan_cycle_time", scan_cycle_time, 0.040); //SICK-docu says S300 scans every 40ms
syncedSICKStamp = 0;
syncedROSTime = ros::Time::now();
syncedTimeReady = false;
// implementation of topics to publish
topicPub_LaserScan = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
topicPub_Diagnostic_ = nh.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
// implementation of topics to subscribe
//--
// implementation of service servers
//--
}
bool TaskInverseKinematics::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
{
nh_ = n;
// get URDF and name of root and tip from the parameter server
std::string robot_description, root_name, tip_name;
if (!ros::param::search(n.getNamespace(),"robot_description", robot_description))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No robot description (URDF) found on parameter server ("<<n.getNamespace()<<"/robot_description)");
return false;
}
if (!nh_.getParam("root_name", root_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No root name found on parameter server ("<<n.getNamespace()<<"/root_name)");
return false;
}
if (!nh_.getParam("tip_name", tip_name))
{
ROS_ERROR_STREAM("TaskInverseKinematics: No tip name found on parameter server ("<<n.getNamespace()<<"/tip_name)");
return false;
}
ROS_INFO("robot_description: %s",robot_description.c_str());
ROS_INFO("root_name: %s",root_name.c_str());
ROS_INFO("tip_name: %s",tip_name.c_str());
// Get the gravity vector (direction and magnitude)
KDL::Vector gravity_ = KDL::Vector::Zero();
gravity_(2) = -9.81;
// Construct an URDF model from the xml string
std::string xml_string;
if (n.hasParam(robot_description))
n.getParam(robot_description.c_str(), xml_string);
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", robot_description.c_str());
n.shutdown();
return false;
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",robot_description.c_str());
n.shutdown();
return false;
}
ROS_DEBUG("%s content\n%s", robot_description.c_str(), xml_string.c_str());
// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n.shutdown();
return false;
}
ROS_DEBUG("Successfully parsed urdf file");
KDL::Tree kdl_tree_;
if (!kdl_parser::treeFromUrdfModel(model, kdl_tree_))
{
ROS_ERROR("Failed to construct kdl tree");
n.shutdown();
return false;
}
// Populate the KDL chain
if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_name<<" --> "<<tip_name);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree_.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree_.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin(); it != segment_map.end(); it++ )
ROS_ERROR_STREAM( " "<<(*it).first);
return false;
}
ROS_DEBUG("Number of segments: %d", kdl_chain_.getNrOfSegments());
ROS_DEBUG("Number of joints in chain: %d", kdl_chain_.getNrOfJoints());
// Parsing joint limits from urdf model
boost::shared_ptr<const urdf::Link> link_ = model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint_;
joint_limits_.min.resize(kdl_chain_.getNrOfJoints());
joint_limits_.max.resize(kdl_chain_.getNrOfJoints());
joint_limits_.center.resize(kdl_chain_.getNrOfJoints());
int index;
std::cout<< "kdl_chain_.getNrOfJoints(): " << kdl_chain_.getNrOfJoints() << std::endl;
for (int i = 0; i < kdl_chain_.getNrOfJoints() && link_; i++)
{
joint_ = model.getJoint(link_->parent_joint->name);
index = kdl_chain_.getNrOfJoints() - i - 1;
joint_limits_.min(index) = joint_->limits->lower;
joint_limits_.max(index) = joint_->limits->upper;
joint_limits_.center(index) = (joint_limits_.min(index) + joint_limits_.max(index))/2;
link_ = model.getLink(link_->getParent()->name);
}
ROS_INFO("Successfully parsed joint limits");
// Get joint handles for all of the joints in the chain
for(std::vector<KDL::Segment>::const_iterator it = kdl_chain_.segments.begin()+1; it != kdl_chain_.segments.end(); ++it)
{
joint_handles_.push_back(robot->getHandle(it->getJoint().getName()));
ROS_DEBUG("%s", it->getJoint().getName().c_str() );
}
ROS_DEBUG(" Number of joints in handle = %lu", joint_handles_.size() );
PIDs_.resize(kdl_chain_.getNrOfJoints());
// Parsing PID gains from YAML
std::string pid_ = ("pid_");
for (int i = 0; i < joint_handles_.size(); ++i)
{
if (!PIDs_[i].init(ros::NodeHandle(n, pid_ + joint_handles_[i].getName())))
{
ROS_ERROR("Error initializing the PID for joint %d",i);
return false;
}
}
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
id_solver_.reset(new KDL::ChainDynParam(kdl_chain_,gravity_));
fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_,joint_limits_.min,joint_limits_.max,*fk_pos_solver_,*ik_vel_solver_));
joint_msr_states_.resize(kdl_chain_.getNrOfJoints());
joint_des_states_.resize(kdl_chain_.getNrOfJoints());
tau_cmd_.resize(kdl_chain_.getNrOfJoints());
J_.resize(kdl_chain_.getNrOfJoints());
sub_command_ = nh_.subscribe("command_configuration", 1, &TaskInverseKinematics::command_configuration, this);
return true;
}
/**\fn int init_ravengains(ros::NodeHandle n, struct device *device0)
\brief Get ravengains from ROS parameter server.
\struct device
\param device0 pointer to device struct
\warning The order of gains in the parameter is very important.
* Make sure that numerical order of parameters matches the numerical order of the dof types
\post dof_types[].kp and dof_types[].kd have been set from ROS parameters
\return
\ingroup DataStructures
*/
int init_ravengains(ros::NodeHandle n, struct device *device0)
{
XmlRpc::XmlRpcValue kp_green, kp_gold, kd_green, kd_gold, ki_green, ki_gold;
bool res=0;
log_msg("Getting gains params...");
// initialize all gains to zero
for (int i = 0; i < MAX_MECH * MAX_DOF_PER_MECH; i++)
{
DOF_types[i].KP = DOF_types[i].KD = DOF_types[i].KI = 0.0;
}
/// Get gains from parameter server, use default value of 0.0 if parameters ain't found
if (n.hasParam("/gains_gold_kp") &&
n.hasParam("/gains_gold_kd") &&
n.hasParam("/gains_gold_ki") &&
n.hasParam("/gains_green_kp") &&
n.hasParam("/gains_green_kd") &&
n.hasParam("/gains_green_ki") )
{
res = n.getParam("/gains_green_kp", kp_green);
res &= n.getParam("/gains_green_kd", kd_green);
res &= n.getParam("/gains_green_ki", ki_green);
res &= n.getParam("/gains_gold_kp", kp_gold);
res &= n.getParam("/gains_gold_kd", kd_gold);
res &= n.getParam("/gains_gold_ki", ki_gold);
}
// Did we get the gains??
if ( !res ||
(kp_green.size() != MAX_DOF_PER_MECH) ||
(kd_green.size() != MAX_DOF_PER_MECH) ||
(ki_green.size() != MAX_DOF_PER_MECH) ||
(kp_gold.size() != MAX_DOF_PER_MECH) ||
(kd_gold.size() != MAX_DOF_PER_MECH) ||
(ki_gold.size() != MAX_DOF_PER_MECH) )
{
ROS_ERROR("Gains parameters failed. Setting zero gains");
}
else
{
bool initgold=0, initgreen=0;
for (int i = 0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
int dofindex;
// Set gains for gold and green arms
if ( device0->mech[i].type == GOLD_ARM)
{
initgold=true;
dofindex = j;
DOF_types[dofindex].KP = (double)kp_gold[j]; // Cast XMLRPC value to a double and set gain
DOF_types[dofindex].KD = (double)kd_gold[j]; // ""
DOF_types[dofindex].KI = (double)ki_gold[j]; // ""
}
else if ( device0->mech[i].type == GREEN_ARM)
{
initgreen=true;
dofindex = 1 * MAX_DOF_PER_MECH + j;
DOF_types[dofindex].KP = (double)kp_green[j]; // ""
DOF_types[dofindex].KD = (double)kd_green[j]; // ""
DOF_types[dofindex].KI = (double)ki_green[j]; // ""
}
else
{
ROS_ERROR("What device is this?? %d\n",device0->mech[i].type);
}
}
}
if (!initgold){
ROS_ERROR("Failed to set gains for gold arm (ser:%d not %d). Set to zero", device0->mech[0].type, GOLD_ARM);
}
if (!initgreen){
ROS_ERROR("Failed to set gains for green arm (ser:%d not %d). Set to zero", device0->mech[1].type, GREEN_ARM);
}
log_msg(" PD gains set to");
log_msg(" gold: %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf",
DOF_types[0].KP, DOF_types[0].KD, DOF_types[0].KI,
DOF_types[1].KP, DOF_types[1].KD, DOF_types[1].KI,
DOF_types[2].KP, DOF_types[2].KD, DOF_types[2].KI,
DOF_types[3].KP, DOF_types[3].KD, DOF_types[3].KI,
DOF_types[4].KP, DOF_types[4].KD, DOF_types[4].KI,
DOF_types[5].KP, DOF_types[5].KD, DOF_types[5].KI,
DOF_types[6].KP, DOF_types[6].KD, DOF_types[6].KI,
DOF_types[7].KP, DOF_types[7].KD, DOF_types[7].KI);
log_msg(" green: %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf, %.3lf/%.3lf/%.3lf",
DOF_types[8].KP, DOF_types[8].KD, DOF_types[8].KI,
DOF_types[9].KP, DOF_types[9].KD, DOF_types[9].KI,
DOF_types[10].KP, DOF_types[10].KD, DOF_types[10].KI,
DOF_types[11].KP, DOF_types[11].KD, DOF_types[11].KI,
DOF_types[12].KP, DOF_types[12].KD, DOF_types[12].KI,
DOF_types[13].KP, DOF_types[13].KD, DOF_types[13].KI,
DOF_types[14].KP, DOF_types[14].KD, DOF_types[14].KI,
DOF_types[15].KP, DOF_types[15].KD, DOF_types[15].KI);
}
return 0;
}
bool DiffDriveController::setOdomParamsFromUrdf(ros::NodeHandle& root_nh,
const std::string& left_wheel_name,
const std::string& right_wheel_name,
bool lookup_wheel_separation,
bool lookup_wheel_radius)
{
if (!(lookup_wheel_separation || lookup_wheel_radius))
{
// Short-circuit in case we don't need to look up anything, so we don't have to parse the URDF
return true;
}
// Parse robot description
const std::string model_param_name = "robot_description";
bool res = root_nh.hasParam(model_param_name);
std::string robot_model_str="";
if (!res || !root_nh.getParam(model_param_name,robot_model_str))
{
ROS_ERROR_NAMED(name_, "Robot descripion couldn't be retrieved from param server.");
return false;
}
boost::shared_ptr<urdf::ModelInterface> model(urdf::parseURDF(robot_model_str));
boost::shared_ptr<const urdf::Joint> left_wheel_joint(model->getJoint(left_wheel_name));
boost::shared_ptr<const urdf::Joint> right_wheel_joint(model->getJoint(right_wheel_name));
if (lookup_wheel_separation)
{
// Get wheel separation
if (!left_wheel_joint)
{
ROS_ERROR_STREAM_NAMED(name_, left_wheel_name
<< " couldn't be retrieved from model description");
return false;
}
if (!right_wheel_joint)
{
ROS_ERROR_STREAM_NAMED(name_, right_wheel_name
<< " couldn't be retrieved from model description");
return false;
}
ROS_INFO_STREAM("left wheel to origin: " << left_wheel_joint->parent_to_joint_origin_transform.position.x << ","
<< left_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
<< left_wheel_joint->parent_to_joint_origin_transform.position.z);
ROS_INFO_STREAM("right wheel to origin: " << right_wheel_joint->parent_to_joint_origin_transform.position.x << ","
<< right_wheel_joint->parent_to_joint_origin_transform.position.y << ", "
<< right_wheel_joint->parent_to_joint_origin_transform.position.z);
wheel_separation_ = euclideanOfVectors(left_wheel_joint->parent_to_joint_origin_transform.position,
right_wheel_joint->parent_to_joint_origin_transform.position);
}
if (lookup_wheel_radius)
{
// Get wheel radius
if (!getWheelRadius(model->getLink(left_wheel_joint->child_link_name), wheel_radius_))
{
ROS_ERROR_STREAM_NAMED(name_, "Couldn't retrieve " << left_wheel_name << " wheel radius");
return false;
}
}
return true;
}
void CommonDataSubscriber::setupCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
const std::string & name)
{
bool subscribeScan2d = false;
bool subscribeScan3d = false;
bool subscribeOdomInfo = false;
bool subscribeUserData = false;
bool subscribeOdom = true;
int rgbdCameras = 1;
name_ = name;
// ROS related parameters (private)
pnh.param("subscribe_depth", subscribedToDepth_, subscribedToDepth_);
if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
{
ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
}
pnh.param("subscribe_rgb", subscribedToRGB_, subscribedToRGB_);
pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
pnh.param("subscribe_stereo", subscribedToStereo_, subscribedToStereo_);
pnh.param("subscribe_rgbd", subscribedToRGBD_, subscribedToRGBD_);
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
pnh.param("subscribe_odom", subscribeOdom, subscribeOdom);
if(subscribedToDepth_ && subscribedToStereo_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
subscribedToRGB_ = false;
}
if(subscribedToRGB_ && subscribedToStereo_)
{
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgb cannot be true at the same time. Parameter subscribe_rgb is set to false.");
subscribedToRGB_ = false;
}
if(subscribedToDepth_ && subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
subscribedToDepth_ = false;
subscribedToRGB_ = false;
}
if(subscribedToRGB_ && subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_rgb and subscribe_rgbd cannot be true at the same time. Parameter subscribe_rgb is set to false.");
subscribedToRGB_ = false;
}
if(subscribedToStereo_ && subscribedToRGBD_)
{
ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
subscribedToStereo_ = false;
}
if(subscribeScan2d && subscribeScan3d)
{
ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
subscribeScan3d = false;
}
if(subscribeScan2d || subscribeScan3d)
{
if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_ && !subscribedToRGB_)
{
approxSync_ = false; // default for scan: exact sync
}
}
if(subscribedToStereo_)
{
approxSync_ = false; // default for stereo: exact sync
}
std::string odomFrameId;
pnh.getParam("odom_frame_id", odomFrameId);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
if(pnh.hasParam("depth_cameras"))
{
ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
}
pnh.param("queue_size", queueSize_, queueSize_);
if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
{
ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
"to \"approx_sync\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("stereo_approx_sync", approxSync_, approxSync_);
}
else
{
pnh.param("approx_sync", approxSync_, approxSync_);
}
if(rgbdCameras <= 0 && subscribedToRGBD_)
{
rgbdCameras = 1;
}
ROS_INFO("%s: subscribe_depth = %s", name.c_str(), subscribedToDepth_?"true":"false");
ROS_INFO("%s: subscribe_rgb = %s", name.c_str(), subscribedToRGB_?"true":"false");
ROS_INFO("%s: subscribe_stereo = %s", name.c_str(), subscribedToStereo_?"true":"false");
ROS_INFO("%s: subscribe_rgbd = %s (rgbd_cameras=%d)", name.c_str(), subscribedToRGBD_?"true":"false", rgbdCameras);
ROS_INFO("%s: subscribe_odom_info = %s", name.c_str(), subscribeOdomInfo?"true":"false");
ROS_INFO("%s: subscribe_user_data = %s", name.c_str(), subscribeUserData?"true":"false");
ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false");
ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
ROS_INFO("%s: queue_size = %d", name.c_str(), queueSize_);
ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false");
subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
if(subscribedToDepth_)
{
setupDepthCallbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(subscribedToStereo_)
{
setupStereoCallbacks(
nh,
pnh,
subscribedToOdom_,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(subscribedToRGB_)
{
setupRGBCallbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(subscribedToRGBD_)
{
if(rgbdCameras == 4)
{
setupRGBD4Callbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(rgbdCameras == 3)
{
setupRGBD3Callbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(rgbdCameras == 2)
{
setupRGBD2Callbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else
{
setupRGBDCallbacks(
nh,
pnh,
subscribedToOdom_,
subscribeUserData,
subscribeScan2d,
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
}
else if(subscribeScan2d || subscribeScan3d)
{
setupScanCallbacks(
nh,
pnh,
subscribeScan2d,
subscribedToOdom_,
subscribeUserData,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
else if(subscribedToOdom_)
{
setupOdomCallbacks(
nh,
pnh,
subscribeUserData,
subscribeOdomInfo,
queueSize_,
approxSync_);
}
if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToRGB_ || subscribedToOdom_)
{
warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
ROS_INFO("%s", subscribedTopicsMsg_.c_str());
}
}
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);
}
}
