int main(int argc, char **argv)
{
ros::init(argc, argv, "sick_tim551_2050001");
// check for TCP - use if ~hostname is set.
ros::NodeHandle nhPriv("~");
bool useTCP = false;
std::string hostname;
if(nhPriv.getParam("hostname", hostname)) {
useTCP = true;
}
sick_tim3xx::SickTim5512050001Parser* parser = new sick_tim3xx::SickTim5512050001Parser();
sick_tim3xx::SickTim3xxCommon* s = NULL;
if(useTCP)
s = new sick_tim3xx::SickTim3xxCommonTcp(hostname, parser);
else
s = new sick_tim3xx::SickTim3xxCommonUsb(parser);
int result = s->init();
while (ros::ok() && (result == EXIT_SUCCESS))
{
ros::spinOnce();
result = s->loopOnce();
}
delete s;
return result;
}
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) {
ros::init(argc, argv, "spatial_perspective_2");
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
yarp::os::Network yarp;
(void) yarp;
SpatialPerspectiveLevel2 node(nh, nhPriv);
(void) node;
ros::spin();
return 0;
}
ControlHead::ControlHead(moveit::planning_interface::MoveGroup* head_group) :
head_group_(head_group),
head_pitch_("head_tilt_joint"),
head_yaw_("head_pan_joint"),
head_step_(0.15)
{
ros::NodeHandle nhPriv("~");
srvHeadPitchDown_ = nhPriv.advertiseService(
"head_pitch_down", &ControlHead::headPitchDown, this);
srvHeadPitchUp_ = nhPriv.advertiseService(
"head_pitch_up", &ControlHead::headPitchUp, this);
srvHeadPitchStraight_ = nhPriv.advertiseService(
"head_pitch_straight", &ControlHead::headPitchStraight, this);
srvHeadPitchDegrees_ = nhPriv.advertiseService(
"head_pitch_degrees", &ControlHead::headPitchDegrees, this);
srvHeadYawLeft_ = nhPriv.advertiseService(
"head_yaw_left", &ControlHead::headYawLeft, this);
srvHeadYawRight_ = nhPriv.advertiseService(
"head_yaw_right", &ControlHead::headYawRight, this);
srvHeadYawStraight_ = nhPriv.advertiseService(
"head_yaw_straight", &ControlHead::headYawStraight, this);
srvHeadYawDegrees_ = nhPriv.advertiseService(
"head_yaw_degrees", &ControlHead::headYawDegrees, this);
srvHeadInitialPosition_ = nhPriv.advertiseService(
"head_initial_position", &ControlHead::headInitialPosition, this);
ROS_INFO("Waiting for %s services.", ros::this_node::getName().c_str());
ros::Duration timeout = ros::Duration(0.5);
// If one service is not online, we get an info message
ros::service::waitForService("control_robot/head_pitch_down", timeout);
ros::service::waitForService("control_robot/head_pitch_up", timeout);
ros::service::waitForService("control_robot/head_pitch_straight", timeout);
ros::service::waitForService("control_robot/head_pitch_degrees", timeout);
ros::service::waitForService("control_robot/head_yaw_left", timeout);
ros::service::waitForService("control_robot/head_yaw_right", timeout);
ros::service::waitForService("control_robot/head_yaw_straight", timeout);
ros::service::waitForService("control_robot/head_yaw_degrees", timeout);
ros::service::waitForService("control_robot/head_initial_position", timeout);
ROS_INFO("Head control services are Ready!");
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "mental_perspective_transformer");
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
if(!nhPriv.hasParam("fovy")) {
nhPriv.setParam("fovy", 90.0);
}
MentalPerspectiveTransformer transform_node(nh, nhPriv);
(void) transform_node;
TransformedPerspectiveVisualizer vis_node(nh, nhPriv, "human_perspective");
(void) vis_node;
ros::spin();
return 0;
}
OrkToPlanningScene::OrkToPlanningScene() :
actionOrk_("recognize_objects", true),
actionOrkToPlanningScene_(ros::NodeHandle(), "ork_to_planning_scene",
boost::bind(&OrkToPlanningScene::orkToPlanningSceneCallback, this, _1), false)
{
ros::NodeHandle nh;
srvObjectInfo_ = nh.serviceClient<object_recognition_msgs::GetObjectInformation>("get_object_info");
srvPlanningScene_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
pubPlanningScene_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
pubMarker_ = nh.advertise<visualization_msgs::MarkerArray>("ork_to_planning_scene_markers", 5);
ROS_INFO("Waiting for recognize_objects action server to start.");
actionOrk_.waitForServer();
ROS_INFO("Waiting for get_object_info service.");
srvObjectInfo_.waitForExistence();
ROS_INFO("Waiting for %s service.", move_group::GET_PLANNING_SCENE_SERVICE_NAME.c_str());
srvPlanningScene_.waitForExistence();
ROS_INFO("Waiting for planning_scene publisher to have subscribers.");
while(pubPlanningScene_.getNumSubscribers() < 1) {
ros::Duration(0.5).sleep();
}
ros::NodeHandle nhPriv("~");
nhPriv.param("object_match_distance", object_match_distance_, 0.15);
nhPriv.param("table_match_distance", table_match_distance_, 0.1);
nhPriv.param("object_z_match_distance", object_z_match_distance_, 0.15);
nhPriv.param("table_z_match_distance", table_z_match_distance_, 0.1);
nhPriv.param("table_min_area", table_min_area_, 0.16);
nhPriv.param("table_min_z", table_min_z_, 0.3);
nhPriv.param("table_max_z", table_max_z_, 1.1);
nhPriv.param("table_thickness", table_thickness_, 0.075);
add_objects_ = true;
ROS_INFO("Actions and Services available - spinning up ork_to_planning_scene action.");
actionOrkToPlanningScene_.start();
ROS_INFO("Ready!");
}
ControlTorso::ControlTorso(moveit::planning_interface::MoveGroup* torso_group) :
torso_group_(torso_group),
torso_("torso_lift_joint")
{
ros::NodeHandle nhPriv("~");
srvTorsoLiftMax_ = nhPriv.advertiseService(
"torso_lift_max", &ControlTorso::torsoLiftMax, this);
srvTorsoLiftMin_ = nhPriv.advertiseService(
"torso_lift_min", &ControlTorso::torsoLiftMin, this);
srvTorsoLift_ = nhPriv.advertiseService(
"torso_lift", &ControlTorso::torsoLift, this);
ROS_INFO("Waiting for %s services.", ros::this_node::getName().c_str());
ros::Duration timeout = ros::Duration(0.5);
// If one service is not online, we get an info message
ros::service::waitForService("control_robot/torso_lift_max", timeout);
ros::service::waitForService("control_robot/torso_lift_min", timeout);
ros::service::waitForService("control_robot/torso_lift", timeout);
ROS_INFO("Torso control services are Ready!");
}
int main (int argc, char** argv)
{
ros::init (argc, argv, "sick_tim310s01");
ros::NodeHandle nhPriv ("~");
bool subscribe_datagram;
nhPriv.param ("subscribe_datagram", subscribe_datagram, false);
sick_tim::SickTim310S01Parser* parser = new sick_tim::SickTim310S01Parser();
sick_tim::SickTimCommon* s = NULL;
int result = EXIT_FAILURE;
while (ros::ok())
{
// Atempt to connect/reconnect
delete s;
if (subscribe_datagram)
s = new sick_tim::SickTimCommonMockup (parser);
else
s = new sick_tim::SickTimCommonUsb (parser);
result = s->init();
while (ros::ok() && (result == EXIT_SUCCESS))
{
ros::spinOnce();
result = s->loopOnce();
}
if (ros::ok() && !subscribe_datagram)
ros::Duration (1.0).sleep(); // Only attempt USB connections once per second
}
delete s;
delete parser;
return result;
}
int main (int argc, char **argv)
{
ros::init(argc, argv, "estop_safety_controller");
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
bool auto_estop = true;
nhPriv.param("auto_estop", auto_estop, auto_estop);
bool use_kobuki_din3_estop = false;
nhPriv.param("use_kobuki_din3_estop", use_kobuki_din3_estop, use_kobuki_din3_estop);
ros::Subscriber sub = nh.subscribe ("estop", 1, estop);
ros::Subscriber subKobuki;
if(use_kobuki_din3_estop)
subKobuki = nh.subscribe("mobile_base/sensors/core", 1, kobuki_core);
pub = nh.advertise<geometry_msgs::Twist>("cmd_vel_mux/input/estop", 1);
ROS_INFO("estop_safety_controller: auto_estop: %s use_kobuki_din3_estop: %s",
auto_estop ? "enabled" : "disabled", use_kobuki_din3_estop ? "enabled" : "disabled");
ros::Rate r(10);
while (ros::ok()) {
ros::Duration timeSinceLastData = ros::Time::now() - lastDataTime;
if(timeSinceLastData > ros::Duration(1.0)) {
if(auto_estop) {
ROS_WARN_THROTTLE(1.0, "No EStop data available - Stopping robot!");
stop_robot();
} else {
ROS_WARN_THROTTLE(5.0, "No EStop data available!");
}
}
ros::spinOnce();
r.sleep();
}
}
int main (int argc, char** argv) {
if (argc < 2) {
ROS_ERROR("Not enough arguments: raytracer <pointcloud_topic>");
return -1;
}
ros::init(argc, argv, "raytracer");
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
if(!nhPriv.hasParam("fovy")) {
nhPriv.setParam("fovy", 50.0);
}
yarp::os::Network yarp;
(void) yarp;
std::string pointcloud_topic = argv[1];
Raytracer node(nh, nhPriv, pointcloud_topic);
(void) node;
ros::spin();
return 0;
}
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);
ROS_ASSERT(datumConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(datumConfig.size() == 4);
useManualDatum_ = true;
std::ostringstream ostr;
ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] << " " << datumConfig[3];
std::istringstream istr(ostr.str());
istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_;
robot_localization::SetDatum::Request request;
request.geo_pose.position.latitude = datumLat;
request.geo_pose.position.longitude = datumLon;
request.geo_pose.position.altitude = 0.0;
tf2::Quaternion quat;
quat.setRPY(0.0, 0.0, datumYaw);
request.geo_pose.orientation = tf2::toMsg(quat);
robot_localization::SetDatum::Response response;
datumCallback(request, response);
}
catch (XmlRpc::XmlRpcException &e)
{
ROS_ERROR_STREAM("ERROR reading sensor config: " << e.getMessage() <<
" for process_noise_covariance (type: " << datumConfig.getType() << ")");
}
}
int main (int argc, char** argv)
{
ros::init (argc, argv, "sick_tim551_2050001");
ros::NodeHandle nhPriv ("~");
// check for TCP - use if ~hostname is set.
bool useTCP = false;
std::string hostname;
if (nhPriv.getParam ("hostname", hostname))
{
useTCP = true;
}
std::string port;
nhPriv.param<std::string> ("port", port, "2112");
int timelimit;
nhPriv.param ("timelimit", timelimit, 5);
bool subscribe_datagram;
nhPriv.param ("subscribe_datagram", subscribe_datagram, false);
sick_tim::SickTim5512050001Parser* parser = new sick_tim::SickTim5512050001Parser();
double param;
if (nhPriv.getParam ("range_min", param))
{
parser->set_range_min (param);
}
if (nhPriv.getParam ("range_max", param))
{
parser->set_range_max (param);
}
if (nhPriv.getParam ("time_increment", param))
{
parser->set_time_increment (param);
}
sick_tim::SickTimCommon* s = NULL;
int result = EXIT_FAILURE;
while (ros::ok())
{
// Atempt to connect/reconnect
delete s;
if (subscribe_datagram)
s = new sick_tim::SickTimCommonMockup (parser);
else if (useTCP)
s = new sick_tim::SickTimCommonTcp (hostname, port, timelimit, parser);
else
s = new sick_tim::SickTimCommonUsb (parser);
result = s->init();
while (ros::ok() && (result == EXIT_SUCCESS))
{
ros::spinOnce();
result = s->loopOnce();
}
if (ros::ok() && !subscribe_datagram && !useTCP)
ros::Duration (1.0).sleep(); // Only attempt USB connections once per second
}
delete s;
delete parser;
return result;
}
void CalibrateAction::executeCB(const calibrate_twist::CalibrateGoalConstPtr &goal)
{
success = true;
// read all necessary parameters
ros::NodeHandle nhPriv("~");
nhPriv.getParamCached("odo_cache_depths", odo_cache_depths);
nhPriv.getParamCached("stability_timeout", stability_timeout);
nhPriv.getParamCached("stability_intervalDuration", stability_intervalDuration);
nhPriv.getParamCached("stability_xThreshold", stability_xThreshold);
nhPriv.getParamCached("stability_zThreshold", stability_zThreshold);
nhPriv.getParamCached("calibration_calc_interval", calibration_calc_interval);
nhPriv.getParamCached("tfFixedFrame", tfFixedFrame);
nhPriv.getParamCached("robotFrame", robotFrame);
nhPriv.getParamCached("cmdVelTopic", cmdVelTopic);
nhPriv.getParamCached("minStabilityDuration", minStabilityDuration);
nhPriv.getParamCached("transforms_interval_size", transforms_interval_size);
nhPriv.getParamCached("cal_costmap", cal_costmap);
nhPriv.getParamCached("traj_sim_granularity_", traj_sim_granularity_);
nhPriv.getParamCached("traj_dist_threshold", traj_dist_threshold);
nhPriv.getParamCached("accel_max_x", accel_max_x);
nhPriv.getParamCached("accel_max_y", accel_max_y);
nhPriv.getParamCached("accel_max_theta", accel_max_theta);
nhPriv.getParamCached("min_time_clear", min_time_clear);
goal_ = *goal;
listener = new tf::TransformListener((goal_.duration)*2); // set cache time twice the time of the calibr. run
cost_map = new costmap_2d::Costmap2DROS(cal_costmap, *listener);
voronoi_.initializeEmpty(cost_map->getSizeInCellsX(), cost_map->getSizeInCellsY(), true);
//ros::Subscriber cm_sub = nh_.subscribe("~/move_base/local_costmap/obstacles", 1, CalibrateAction::costmapCB);
message_filters::Subscriber<nav_msgs::Odometry> sub(nh_, "/odom", 1);
odo_cache = new message_filters::Cache<nav_msgs::Odometry> (sub, odo_cache_depths);
estTraj_pub = nh_.advertise<geometry_msgs::PoseArray>("est_traj_", 10);
calcTraj_pub = nh_.advertise<geometry_msgs::PoseArray>("calc_traj_", 10);
twist_pub = nh_.advertise<geometry_msgs::Twist>(cmdVelTopic,1);
voronoi_pub = nh_.advertise<visualization_msgs::MarkerArray>("voronoi_marker", 10);
// necessary? how to initialize as empty?
zero_twist.angular.x = 0;
zero_twist.angular.y = 0;
zero_twist.angular.z = 0;
zero_twist.linear.x = 0;
zero_twist.linear.y = 0;
zero_twist.linear.z = 0;
// publish info to the console for the user
ROS_INFO("%s: Executing, running calibration run for %f seconds with linear speed %f, angular speed %f", action_name_.c_str(), goal_.duration.toSec(), goal_.twist_goal.linear.x, goal_.twist_goal.angular.z);
// update voronoi at first, else no distance check is possible
updateVoronoi(); // load values from costmap and push them into voronoi
//**************************************************
// bringing the robot to the goal speed
//**************************************************
bool stability_reached = false;
bool check = estimateFullPathIsClear(goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, goal_.duration.toSec(), accel_max_x, accel_max_y, accel_max_theta);
ROS_INFO("Full check results in %i", check);
if(checkPath(0, 0, 0, goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, accel_max_x, accel_max_y, accel_max_theta))
{
stability_reached = bringupGoalSpeed();
}
// not stopping robot here because we want to have continuous movement!
if(!stability_reached)
{
twist_pub.publish(zero_twist); // safety first, stop robot
as_.setAborted();
ROS_INFO("%s: Aborted. No stability reached within timeout", action_name_.c_str());
success = false;
return;
}
//**************************************************
// starting calibration run for given duration
//**************************************************
else if(success)
{
ROS_INFO("Speed up successful, starting calibration run");
if(checkPath(goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, goal_.duration.toSec(),
goal_.twist_goal.linear.x, goal_.twist_goal.linear.y, goal_.twist_goal.angular.z, 0.0, 0.0, 0.0))
{
success = startCalibrationRun(); // check whether we finished the run successfully or not
}
else
{
twist_pub.publish(zero_twist); // safety first, stop robot
as_.setAborted();
ROS_INFO("%s: Aborted. No space to drive planned trajectory", action_name_.c_str());
success = false;
return;
}
}
// end of movement, therefore robo is stopped
twist_pub.publish(zero_twist); // safety first, stop robot
//**************************************************
// calculating the result
//**************************************************
if(success)
{
ROS_INFO("Calibration run finished, calculating result...");
calculateResult();
}
else
{
ROS_INFO("Calibration aborted");
as_.setAborted();
return;
}
//**************************************************
// publishing the result
//**************************************************
if(success)
{
result_.calibrated_result = twistWCFromTf;
result_.odo_result = twistWCFromOdometry;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
// callback finished
}
void NavSatTransform::run()
{
ros::Time::init();
double frequency = 10;
ros::NodeHandle nh;
ros::NodeHandle nhPriv("~");
// Subscribe to the messages we need
ros::Subscriber odomSub = nh.subscribe("odometry/filtered", 1, &NavSatTransform::odomCallback, this);
ros::Subscriber gpsSub = nh.subscribe("gps/fix", 1, &NavSatTransform::gpsFixCallback, this);
ros::Subscriber imuSub = nh.subscribe("imu/data", 1, &NavSatTransform::imuCallback, this);
// Load the parameters we need
nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
nhPriv.getParam("yaw_offset", yawOffset_);
nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
nhPriv.param("zero_altitude", zeroAltitude_, false);
nhPriv.param("publish_filtered_gps", publishGps_, false);
nhPriv.param("frequency", frequency, 10.0);
ros::Publisher gpsOdomPub = nh.advertise<nav_msgs::Odometry>("odometry/gps", 10);
ros::Publisher filteredGpsPub;
if(publishGps_)
{
filteredGpsPub = nh.advertise<sensor_msgs::NavSatFix>("gps/filtered", 10);
}
tf::TransformBroadcaster utmBroadcaster;
tf::StampedTransform utmTransformStamped;
utmTransformStamped.child_frame_id_ = "utm";
ros::Rate rate(frequency);
while(ros::ok())
{
ros::spinOnce();
if(!transformGood_)
{
computeTransform();
if(transformGood_)
{
// Once we have the transform, we don't need the IMU
imuSub.shutdown();
}
}
else
{
nav_msgs::Odometry gpsOdom;
if(prepareGpsOdometry(gpsOdom))
{
gpsOdomPub.publish(gpsOdom);
}
if(publishGps_)
{
sensor_msgs::NavSatFix odomGps;
if(prepareFilteredGps(odomGps))
{
filteredGpsPub.publish(odomGps);
}
}
// Send out the UTM transform in case anyone
// else would like to use it.
if(transformGood_ && broadcastUtmTransform_)
{
utmTransformStamped.setData(utmWorldTransform_);
utmTransformStamped.frame_id_ = worldFrameId_;
utmTransformStamped.stamp_ = ros::Time::now();
utmBroadcaster.sendTransform(utmTransformStamped);
}
}
rate.sleep();
}
}
StateCreatorRobotPose::StateCreatorRobotPose()
{
ros::NodeHandle nhPriv("~");
ros::NodeHandle nh;
nhPriv.getParam("cell_size", cell_size);
nhPriv.getParam("grid_size", grid_size);
nhPriv.getParam("robotLoc", robotLoc);
nhPriv.getParam("robotDir", robotDir);
nhPriv.getParam("goalLoc", goalLoc);
nhPriv.getParam("boxLocs", xmlboxLocs);
nhPriv.getParam("ballLocs", xmlballLocs);
nhPriv.getParam("boxes", xmlboxes);
nhPriv.getParam("balls", xmlballs);
ROS_ASSERT(xmlboxLocs.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(xmlballLocs.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(xmlboxes.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(xmlballs.getType() == XmlRpc::XmlRpcValue::TypeArray);
//copying xmlrpc to vector
for(int i=0; i<xmlboxLocs.size(); i++){
boxLocs.push_back(xmlboxLocs[i]);
boxes.push_back(xmlboxes[i]);
}
for(int i=0; i<xmlballLocs.size(); i++){
ballLocs.push_back(xmlballLocs[i]);
balls.push_back(xmlballs[i]);
}
//nhPriv.param("nav_target_tolerance_xy", _goalToleranceXY, 0.5);
//nhPriv.param("nav_target_tolerance_yaw", _goalToleranceYaw, 0.26); //15deg
//bool relative;
/* nhPriv.param("nav_target_tolerance_relative_to_move_base", relative, false);
if(relative) {
// relative mode: 1. get the namespace for base_local_planner
std::string base_local_planner_ns;
if(!nhPriv.getParam("nav_base_local_planner_ns", base_local_planner_ns)) {
ROS_WARN("nav_target_tolerance_relative_to_move_base set, but nav_base_local_planner_ns not set - trying to estimate");
std::string local_planner;
if(!nh.getParam("move_base_node/base_local_planner", local_planner)
&& !nh.getParam("move_base/base_local_planner", local_planner)) {
ROS_ERROR("move_base(_node)/base_local_planner not set - falling back to absolute mode.");
} else {
// dwa_local_planner/DWAPlannerROS -> DWAPlannerROS
std::string::size_type x = local_planner.find_last_of("/");
if(x == std::string::npos)
base_local_planner_ns = local_planner;
else
base_local_planner_ns = local_planner.substr(x + 1);
ROS_INFO("Estimated base_local_planner_ns to %s.", base_local_planner_ns.c_str());
}
}
if(!base_local_planner_ns.empty()) { // success: 2. get the xy_goal_tolerance
double move_base_tol_xy;
if(!nh.getParam(base_local_planner_ns + "/xy_goal_tolerance", move_base_tol_xy)) {
ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
<< (base_local_planner_ns + "/xy_goal_tolerance") << " was not set"
<< " - falling back to absolute mode");
} else { // 2. add move_base's tolerance to our relative tolerance
_goalToleranceXY += move_base_tol_xy;
}
double move_base_tol_yaw;
if(!nh.getParam(base_local_planner_ns + "/yaw_goal_tolerance", move_base_tol_yaw)) {
ROS_ERROR_STREAM("nav_target_tolerance_relative_to_move_base was true, but "
<< (base_local_planner_ns + "/yaw_goal_tolerance") << " was not set"
<< " - falling back to absolute mode");
} else { // 2. add move_base's tolerance to our relative tolerance
_goalToleranceYaw += move_base_tol_yaw;
}
}
}
ROS_INFO("Tolerance for accepting nav goals set to %f m, %f deg.",
_goalToleranceXY, angles::to_degrees(_goalToleranceYaw));
*/
if(s_PublishLocationsAsMarkers) {
_markerPub = nh.advertise<visualization_msgs::MarkerArray>("robot_pose_markers", 5, true);
ROS_INFO("marker topic: %s", _markerPub.getTopic().c_str());
}
}