void CalcMin::chatterCallback(const cob_roboskin::roboskinCAN2 msg)
{
n.setParam("/data1", msg.data1);
n.setParam("/data2", msg.data2);
n.setParam("/data3", msg.data3);
n.setParam("/data4", msg.data4);
}
void processGoal(const geometry_msgs::Pose& start_block_pose, const geometry_msgs::Pose& end_block_pose )
{
// Change the goal constraints on the servos to be less strict, so that the controllers don't die. this is a hack
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/elbow_pitch_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/shoulder_pan_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/gripper_roll_joint/goal", 2); // originally it was 0.45
nh_.setParam("/clam_trajectory_controller/joint_trajectory_action_node/constraints/wrist_pitch_joint/goal", 2); // originally it was 0.45
if( !pickAndPlace(start_block_pose, end_block_pose) )
{
ROS_ERROR_STREAM_NAMED("pick_place_moveit","Pick and place failed");
if(action_server_.isActive()) // Make sure we haven't sent a fake goal
{
// Report failure
result_.success = false;
action_server_.setSucceeded(result_);
}
}
else
{
if(action_server_.isActive()) // Make sure we haven't sent a fake goal
{
// Report success
result_.success = true;
action_server_.setSucceeded(result_);
}
}
// TODO: remove
ros::shutdown();
}
void BallDetector::updateLowThreshold(const geometry_msgs::Vector3::ConstPtr& thresh){
int h = thresh->x-1, s = thresh->y-1, v = thresh->z-1;
ROS_INFO("Updated ball low HSV threshold to h,s,v: %d,%d,%d",h,s,v);
lowThresh = cv::Scalar(h,s,v,0);
nh.setParam("thresh/high/h",h);
nh.setParam("thresh/high/s",s);
nh.setParam("thresh/high/v",v);
}
void BallDetector::updateHighThreshold(const geometry_msgs::Vector3::ConstPtr& thresh){
int h = thresh->x+1, s = thresh->y+1, v = thresh->z+1;
ROS_INFO("Updated ball high HSV threshold to h,s,v: %d,%d,%d",h,s,v);
highThresh = cv::Scalar(h,s,v,0);
nh.setParam("thresh/low/h",h);
nh.setParam("thresh/low/s",s);
nh.setParam("thresh/low/v",v);
}
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;
}
BaseCamera *createCamera()
{
std::string cameraType;
node.param("camera_type", cameraType, std::string("usb"));
node.param("video_device", videoDevice, std::string(""));
std::string resolution;
node.getParam("resolution", resolution);
cameraWidth = std::stoi(resolution.substr(0, resolution.find('x')));
cameraHeight = std::stoi(resolution.substr(resolution.find('x') + 1));
node.param("framerate", framerate, 30);
node.param("media_path", mediaPath, std::string("~/.rospilot/media"));
wordexp_t p;
wordexp(mediaPath.c_str(), &p, 0);
if (p.we_wordc != 1) {
ROS_ERROR("Got too many words when expanding media path: %s",
mediaPath.c_str());
}
else {
mediaPath = p.we_wordv[0];
}
wordfree(&p);
if (videoDevice.size() == 0) {
videoDevice = findCameraDevice();
node.setParam("video_device", videoDevice);
}
if (cameraType == "ptp") {
return new PtpCamera();
}
else if (cameraType == "usb") {
ROS_INFO("Requesting camera res %dx%d", cameraWidth, cameraHeight);
UsbCamera *camera = new UsbCamera(videoDevice, cameraWidth, cameraHeight, framerate);
// Read the width and height, since the camera may have altered it to
// something it supports
cameraWidth = camera->getWidth();
cameraHeight = camera->getHeight();
std::stringstream ss;
ss << cameraWidth << "x" << cameraHeight;
node.setParam("resolution", ss.str());
ROS_INFO("Camera selected res %dx%d", cameraWidth, cameraHeight);
return camera;
}
else {
ROS_FATAL("Unsupported camera type: %s", cameraType.c_str());
return nullptr;
}
}
void PluginPlanner::resetOldParameters(ros::NodeHandle& nh)
{
ROS_INFO("Loading from pre-hydro parameter style");
std::vector < XmlRpc::XmlRpcValue > plugins;
plugins.resize(6);
plugins[0] = "Oscillation"; // discards oscillating motions (assisgns cost -1)
plugins[1] = "Obstacle"; // discards trajectories that move into obstacles
plugins[2] = "GoalAlign"; // prefers trajectories that make the nose go towards (local) nose goal
plugins[3] = "PathAlign"; // prefers trajectories that keep the robot nose on nose path
plugins[4] = "PathDist"; // prefers trajectories on global path
plugins[5] = "GoalDist"; // prefers trajectories that go towards (local) goal, based on wave propagation
XmlArray critics;
critics.setArray(&plugins);
nh.setParam("critics", critics);
move_parameter(nh, "path_distance_bias", "PathAlign/scale", 32.0, false);
move_parameter(nh, "goal_distance_bias", "GoalAlign/scale", 24.0, false);
move_parameter(nh, "path_distance_bias", "PathDist/scale", 32.0);
move_parameter(nh, "goal_distance_bias", "GoalDist/scale", 24.0);
move_parameter(nh, "occdist_scale", "Obstacle/scale", 0.01);
move_parameter(nh, "max_scaling_factor", "Obstacle/max_scaling_factor", 0.2);
move_parameter(nh, "scaling_speed", "Obstacle/scaling_speed", 0.25);
}
// Subscribe to models of interest. Currently, we find and subscribe
// to the first 'laser' model and the first 'position' model. Returns
// 0 on success (both models subscribed), -1 otherwise.
//
// Eventually, we should provide a general way to map stage models onto ROS
// topics, similar to Player .cfg files.
int
StageNode::SubscribeModels()
{
n_.setParam("/use_sim_time", true);
for (size_t r = 0; r < this->positionmodels.size(); r++)
{
if(this->lasermodels[r])
{
this->lasermodels[r]->Subscribe();
}
else
{
ROS_ERROR("no laser");
return(-1);
}
if(this->positionmodels[r])
{
this->positionmodels[r]->Subscribe();
}
else
{
ROS_ERROR("no position");
return(-1);
}
laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
}
clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
return(0);
}
void getOrSetParam(const ros::NodeHandle& nh, std::string paramName, T& ref, T defaultVal) {
if (!nh.getParam(paramName, ref)) {
nh.setParam(paramName, defaultVal);
ref = defaultVal;
ROS_INFO_STREAM("setting " << paramName << " to default value " << defaultVal);
}
}
std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
{
std::string full_param_name;
std::string full_radius_param_name;
std::vector<geometry_msgs::Point> points;
if (nh.searchParam("footprint", full_param_name))
{
XmlRpc::XmlRpcValue footprint_xmlrpc;
nh.getParam(full_param_name, footprint_xmlrpc);
if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString)
{
if (makeFootprintFromString(std::string(footprint_xmlrpc), points))
{
writeFootprintToParam(nh, points);
}
}
else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name);
writeFootprintToParam(nh, points);
}
}
else if (nh.searchParam("robot_radius", full_radius_param_name))
{
double robot_radius;
nh.param(full_radius_param_name, robot_radius, 1.234);
points = makeFootprintFromRadius(robot_radius);
nh.setParam("robot_radius", robot_radius);
}
// Else neither param was found anywhere this knows about, so
// defaults will come from dynamic_reconfigure stuff, set in
// cfg/Costmap2D.cfg and read in this file in reconfigureCB().
return points;
}
int main(int argc, char **argv) {
std::vector<double> state_probability(7, 0.0);
//TODO: Should we think about introducing some "undefined" state ?
//or is it of no use?
//make the robot follow the right wall at the beginning of the algorithm
state current_state = FOLLOW_RIGHT;
state_probability[current_state] = 1.0;
ros::init(argc, argv, "WallFollowerController"); // Name of the node is WallFollowerController
n.setParam("/param_gain", 5.0);
ir_sub = n.subscribe("/sensors/transformed/ADC", 1, ir_readings_update); // Subscribing to the processed ir values topic.
ros::Rate loop_rate(UPDATE_RATE);
// Creates a WallFollower object.
WallFollower wf;
// Runs the initiation method (initializes the variable) on the WallFollower object.
wf.init();
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
// Runs the step method of the wallfollower object, which remembers the state through fields (variables).
wf.step();
}
}
void CalcMin::chatterCallback1(const fts_Omega160::fts msg)
{
int data1;
int data2;
int data3;
int data4;
int stop;
n.setParam("/stop", 0);
req.data = std::vector<int>(8);
req.data[4] = msg.Fx;
req.data[5] = msg.Fy;
req.data[6] = msg.Fz;
if (n.getParam("/data1", data1)){
req.data[0] = data1;
}
if (n.getParam("/data2", data2)){
req.data[1] = data2;
}
if (n.getParam("/data3", data3)){
req.data[2] = data3;
}
if (n.getParam("/data4", data4)){
req.data[3] = data4;
}
if (n.getParam("/stop", stop)){
req.data[7] = stop;
}
pub = n.advertise<std_msgs::Int32MultiArray>("/min_pub",1000);
pub.publish(req);
}
bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
{
mav_frame = static_cast<MAV_FRAME>(req.mav_frame);
const std::string mav_frame_str = utils::to_string(mav_frame);
sp_nh.setParam("mav_frame", mav_frame_str);
res.success = true;
return true;
}
void setParams(const ros::NodeHandle& nh) {
if (LocalConfig::trackbars) {
nh.setParam("min_hue", MIN_HUE);
nh.setParam("max_hue", MAX_HUE);
nh.setParam("min_sat", MIN_SAT);
nh.setParam("max_sat", MAX_SAT);
nh.setParam("min_val", MIN_VAL);
nh.setParam("max_val", MAX_VAL);
} else {
getOrSetParam(nh, "min_hue", MIN_HUE, 160);
getOrSetParam(nh, "max_hue", MAX_HUE, 10);
getOrSetParam(nh, "min_sat", MIN_SAT, 150);
getOrSetParam(nh, "max_sat", MAX_SAT, 255);
getOrSetParam(nh, "min_val", MIN_VAL, 100);
getOrSetParam(nh, "max_val", MAX_VAL, 255);
}
}
/*!
* \brief Executes the service callback for set_operation_mode.
*
* Changes the operation mode.
* \param req Service request
* \param res Service response
*/
bool srvCallback_SetOperationMode( cob_srvs::SetOperationMode::Request &req,
cob_srvs::SetOperationMode::Response &res )
{
ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
n_.setParam("OperationMode", req.operation_mode.data.c_str());
res.success.data = true; // 0 = true, else = false
return true;
}
void InfoPublisher::reset( ros::NodeHandle& nh )
{
// We latch as we only publish once
pub_ = nh.advertise<naoqi_bridge_msgs::StringStamped>( topic_, 1, true );
std::string robot_desc = naoqi::tools::getRobotDescription(robot_);
nh.setParam("/robot_description", robot_desc);
std::cout << "load robot description from file" << std::endl;
is_initialized_ = true;
}
void AmclNode::savePoseToServer()
{
// We need to apply the last transform to the latest odom pose to get
// the latest map pose to store. We'll take the covariance from
// last_published_pose.
tf::Pose map_pose = latest_tf_.inverse() * latest_odom_pose_;
double yaw,pitch,roll;
map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );
private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
private_nh_.setParam("initial_pose_a", yaw);
private_nh_.setParam("initial_cov_xx",
last_published_pose.pose.covariance[6*0+0]);
private_nh_.setParam("initial_cov_yy",
last_published_pose.pose.covariance[6*1+1]);
private_nh_.setParam("initial_cov_aa",
last_published_pose.pose.covariance[6*5+5]);
}
explicit HaarAdaNode(const ros::NodeHandle& nh):
node_(nh)
{
num_class1 = 0;
num_class0 = 0;
num_TP_class1 = 0;
num_FP_class1 = 0;
num_TP_class0 = 0;
num_FP_class0 = 0;
string nn = ros::this_node::getName();
int qs;
if(!node_.getParam(nn + "/Q_Size",qs)){
qs=3;
}
int NS;
if(!node_.getParam(nn + "/num_Training_Samples",NS)){
NS = 350; // default sets asside very little for training
node_.setParam(nn + "/num_Training_Samples",NS);
}
HAC_.setMaxSamples(NS);
if(!node_.getParam(nn + "/RemoveOverlappingRois",remove_overlapping_rois_)){
remove_overlapping_rois_ = false;
}
// Published Messages
pub_rois_ = node_.advertise<Rois>("HaarAdaOutputRois",qs);
pub_Color_Image_ = node_.advertise<Image>("HaarAdaColorImage",qs);
pub_Disparity_Image_= node_.advertise<DisparityImage>("HaarAdaDisparityImage",qs);
// Subscribe to Messages
sub_image_.subscribe(node_,"Color_Image",qs);
sub_disparity_.subscribe(node_, "Disparity_Image",qs);
sub_rois_.subscribe(node_,"input_rois",qs);
// Sync the Synchronizer
approximate_sync_.reset(new ApproximateSync(ApproximatePolicy(qs),
sub_image_,
sub_disparity_,
sub_rois_));
approximate_sync_->registerCallback(boost::bind(&HaarAdaNode::imageCb,
this,
_1,
_2,
_3));
}
void writeFootprintToParam(ros::NodeHandle& nh, const std::vector<geometry_msgs::Point>& footprint)
{
std::ostringstream oss;
bool first = true;
for (unsigned int i = 0; i < footprint.size(); i++)
{
geometry_msgs::Point p = footprint[ i ];
if (first)
{
oss << "[[" << p.x << "," << p.y << "]";
first = false;
}
else
{
oss << ",[" << p.x << "," << p.y << "]";
}
}
oss << "]";
nh.setParam("footprint", oss.str().c_str());
}
void Costmap2DROS::writeFootprintToParam( ros::NodeHandle& nh )
{
ostringstream oss;
bool first = true;
for( unsigned int i = 0; i < unpadded_footprint_.size(); i++ )
{
geometry_msgs::Point& p = unpadded_footprint_[ i ];
if( first )
{
oss << "[[" << p.x << "," << p.y << "]";
first = false;
}
else
{
oss << ",[" << p.x << "," << p.y << "]";
}
}
oss << "]";
nh.setParam( "footprint", oss.str().c_str() );
}
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");
}
void imageCb(const ImageConstPtr& image_msg,
const DisparityImageConstPtr& disparity_msg,
const RoisConstPtr& rois_msg){
bool label_all;
vector<int> L_in;
vector<int> L_out;
vector<Rect> R_in;
vector<Rect> R_out;
string param_name;
string nn = ros::this_node::getName();
string cfnm;
int numSamples;
double temp=0.0;
// check encoding and create an intensity image from disparity image
assert(disparity_msg->image.encoding == image_encodings::TYPE_32FC1);
cv::Mat_<float> dmatrix(disparity_msg->image.height,
disparity_msg->image.width,
(float*) &disparity_msg->image.data[0],
disparity_msg->image.step);
if(!node_.getParam(nn + "/UseMissingDataMask",HDAC_.useMissingDataMask_)){
HDAC_.useMissingDataMask_ = false;
}
// **********************************************************************//
// between these comments lies a hack that accounts for the difference //
// between the focal lengths of the kinect's color camera and ir cameras //
// TODO, parameterize to disable this hack for the stereo data //
bool kinect_disparity_fix;
if(!node_.getParam(nn + "/Kinect_Disparity_Fix",kinect_disparity_fix)){
kinect_disparity_fix = false;
}
if(kinect_disparity_fix){
int nrows = 434;
int ncols = 579;
int row_offset = (dmatrix.rows - nrows)/2;
int col_offset = (dmatrix.cols - ncols)/2;
cv::Mat Scaled_Disparity(nrows,ncols,CV_32FC1);
resize(dmatrix,Scaled_Disparity,cv::Size(ncols,nrows),0,0, CV_INTER_NN );
for(int i=0;i<dmatrix.rows;i++){
for(int j=0;j<dmatrix.cols;j++){
dmatrix.at<float>(i,j) = 0.0;
}
}
for(int i=0;i<nrows;i++){
for(int j=0;j<ncols;j++){
dmatrix.at<float>(i+row_offset,j+col_offset) = Scaled_Disparity.at<float>(i,j);
}
}
}
// **********************************************************************//
// take the region of interest message and create vectors of ROIs and labels
R_in.clear();
L_in.clear();
for(unsigned int i=0;i<rois_msg->rois.size();i++){
int x = rois_msg->rois[i].x;
int y = rois_msg->rois[i].y;
int w = rois_msg->rois[i].width;
int h = rois_msg->rois[i].height;
int l = rois_msg->rois[i].label;
Rect R(x,y,w,h);
R_in.push_back(R);
L_in.push_back(l);
}
// do the work of the node
switch(get_mode()){
case DETECT:
label_all = false;
HDAC_.detect(R_in,L_in,dmatrix,R_out,L_out,label_all);
output_rois_.rois.clear();
output_rois_.header.stamp = image_msg->header.stamp;
output_rois_.header.frame_id = image_msg->header.frame_id;
ROS_INFO("HaarDispAda found %d objects",(int)L_out.size());
for(unsigned int i=0; i<R_out.size();i++){
RoiRect R;
R.x = R_out[i].x;
R.y = R_out[i].y;
R.width = R_out[i].width;
R.height = R_out[i].height;
R.label = L_out[i];
output_rois_.rois.push_back(R);
}
pub_rois_.publish(output_rois_);
pub_Color_Image_.publish(image_msg);
pub_Disparity_Image_.publish(disparity_msg);
break;
case ACCUMULATE:
numSamples = HDAC_.addToTraining(R_in,L_in,dmatrix);
param_name = nn + "/num_Training_Samples";
int NS;
if(node_.getParam(param_name,NS)){
float percent = (float)HDAC_.numSamples_ * 100.0/NS;
ROS_INFO("ACCUMULATING: %6.1f%c",percent,'%');
if(numSamples >= NS){
param_name = nn + "/mode";
node_.setParam(param_name, std::string("train"));
ROS_ERROR("DONE Accumulating, switching to train mode");
}
}
break;
case TRAIN:
param_name = nn + "/classifier_file";
node_.param(param_name,cfnm,std::string("/home/clewis/classifiers/test.xml"));
param_name = nn + "/HaarDispAdaPrior";
node_.getParam(param_name,temp);
HDAC_.HaarDispAdaPrior_ = temp;
ROS_ERROR("Submitting %d Samples to Train ouput= %s",HDAC_.numSamples_,cfnm.c_str());
HDAC_.train(cfnm);
param_name = nn + "/mode";
node_.setParam(nn + "/mode", std::string("evaluate"));
ROS_ERROR("DONE TRAINING, switching to evaluate mode");
break;
case EVALUATE:
{
if(!HDAC_.loaded){
param_name = nn + "/classifier_file";
node_.param(param_name,cfnm,std::string("test.xml"));
ROS_ERROR("HaarDispAda LOADING %s",cfnm.c_str());
HDAC_.load(cfnm);
}
label_all = false;
HDAC_.detect(R_in,L_in,dmatrix,R_out,L_out,label_all);
int total0_in_list=0;
int total1_in_list=0;
int fp_in_list=0;
int tp_in_list=0;
// count the input labels
for(unsigned int i=0; i<R_in.size();i++){
if(L_in[i] == 0 || L_in[i] == -1) total0_in_list++;
if(L_in[i] == 1) total1_in_list++;
}
num_class0 += total0_in_list;
num_class1 += total1_in_list;
// count the output labels which have the correct and incorrect label
for(unsigned int i=0; i<R_out.size();i++){
if(L_out[i] == 1){
tp_in_list++;
}
else fp_in_list++;
}
num_TP_class1 += tp_in_list;
num_FP_class1 += fp_in_list;
num_TP_class0 += total0_in_list - fp_in_list;
num_FP_class0 += total1_in_list - tp_in_list;
float tp1 = (float)num_TP_class1 / (float) num_class1 * 100.0;
float tp0 = (float)num_TP_class0 / (float) num_class0 * 100.0;
float fp1 = (float)num_FP_class1 / (float) num_class1 * 100.0;
float fp0 = (float)num_FP_class0 / (float) num_class0 * 100.0;
ROS_ERROR("TP0 = %6.2f FP0 = %6.2f TP1 = %6.2f FP1 = %6.2f",tp0,fp0,tp1,fp1);
}
// TODO
break;
case LOAD:
param_name = nn + "/classifier_file";
node_.param(param_name,cfnm,std::string("test.xml"));
ROS_ERROR("HaarDispAda LOADING %s",cfnm.c_str());
HDAC_.load(cfnm);
param_name = nn + "/mode";
node_.setParam(param_name, std::string("detect"));
break;
}// end switch
}
bool calibrateSegmentCallback(vicon_bridge::viconCalibrateSegment::Request& req,
vicon_bridge::viconCalibrateSegment::Response& resp)
{
std::string full_name = req.subject_name + "/" + req.segment_name;
ROS_INFO("trying to calibrate %s", full_name.c_str());
SegmentMap::iterator seg_it = segment_publishers_.find(full_name);
if (seg_it == segment_publishers_.end())
{
ROS_WARN("frame %s not found --> not calibrating", full_name.c_str());
resp.success = false;
resp.status = "segment " + full_name + " not found";
return false;
}
SegmentPublisher & seg = seg_it->second;
if (seg.calibrated)
{
ROS_INFO("%s already calibrated, deleting old calibration", full_name.c_str());
seg.calibration_pose.setIdentity();
}
vicon_bridge::viconGrabPose::Request grab_req;
vicon_bridge::viconGrabPose::Response grab_resp;
grab_req.n_measurements = req.n_measurements;
grab_req.subject_name = req.subject_name;
grab_req.segment_name = req.segment_name;
bool ret = grabPoseCallback(grab_req, grab_resp);
if (!ret)
{
resp.success = false;
resp.status = "error while grabbing pose from Vicon";
return false;
}
tf::Transform t;
t.setOrigin(tf::Vector3(grab_resp.pose.pose.position.x, grab_resp.pose.pose.position.y,
grab_resp.pose.pose.position.z - req.z_offset));
t.setRotation(tf::Quaternion(grab_resp.pose.pose.orientation.x, grab_resp.pose.pose.orientation.y,
grab_resp.pose.pose.orientation.z, grab_resp.pose.pose.orientation.w));
seg.calibration_pose = t.inverse();
// write zero_pose to parameter server
string param_suffix(full_name + "/zero_pose/");
nh_priv.setParam(param_suffix + "orientation/w", t.getRotation().w());
nh_priv.setParam(param_suffix + "orientation/x", t.getRotation().x());
nh_priv.setParam(param_suffix + "orientation/y", t.getRotation().y());
nh_priv.setParam(param_suffix + "orientation/z", t.getRotation().z());
nh_priv.setParam(param_suffix + "position/x", t.getOrigin().x());
nh_priv.setParam(param_suffix + "position/y", t.getOrigin().y());
nh_priv.setParam(param_suffix + "position/z", t.getOrigin().z());
ROS_INFO_STREAM("calibration completed");
resp.pose = grab_resp.pose;
resp.success = true;
resp.status = "calibration successful";
seg.calibrated = true;
return true;
}
void CalcMin::chatterCallback(const cob_roboskin::roboskinCAN2 msg)
{
int min1;
int min2;
int min3;
int min4;
if (n.getParam("/min1", min1)){
if( min1 == 0 ){
n.setParam("/min1", 240);
}
else{
if(msg.data1 < min1 ){
n.setParam("/min1", msg.data1);
cout << "new min for id1 " << min1 << "\t";
}
}
}
if (n.getParam("/min2", min2))
{
if( min2 == 0 ){
n.setParam("/min2", 240);
}
else{
if(msg.data2 < min2 ){
n.setParam("/min2", msg.data2);
cout << "new min for id2 " << min2 << "\t";
}
}
}
if (n.getParam("/min3", min3))
{
if( min3 == 0 ){
n.setParam("/min3", 240);
}
else{
if(msg.data3 < min3 ){
n.setParam("/min3", msg.data3);
cout << "new min for id3 " << min3 << "\t";
}
}
}
if (n.getParam("/min4", min4))
{
if( min4 == 0 ){
n.setParam("/min4", 240);
}
else{
if(msg.data4 < min4 ){
n.setParam("/min4", msg.data4);
cout << "new min for id4 " << min4 << "\t";
}
}
}
}
void init_system( ros::NodeHandle& nh )
{
string nodeName = ros::this_node::getName();
string nameSpace = ros::this_node::getNamespace();
string paramStr;
// >>>>> Global
paramStr = ( nameSpace + nodeName + "/general/Telemetry_freq");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, telem_freq);
else
nh.setParam(paramStr, telem_freq );
paramStr = ( nameSpace + nodeName + "/general/Command_timeout");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, vel_cmd_timeout_sec);
else
nh.setParam(paramStr, vel_cmd_timeout_sec );
paramStr = ( nameSpace + nodeName + "/general/Speed_filter_enabled");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, speed_filter_enabled);
else
nh.setParam(paramStr, speed_filter_enabled );
// <<<<< Global
// >>>>> Serial Port
paramStr = ( nameSpace + nodeName + "/rc_serial/Board_Idx");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, boardIdx);
else
nh.setParam(paramStr, boardIdx );
paramStr = ( nameSpace + nodeName + "/rc_serial/Serial_Port");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, serialPort);
else
nh.setParam(paramStr, serialPort );
paramStr = ( nameSpace + nodeName + "/rc_serial/Baud_Rate");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, serialbaudrate);
else
nh.setParam(paramStr, serialbaudrate );
paramStr = ( nameSpace + nodeName + "/rc_serial/Parity");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, parity);
else
nh.setParam(paramStr, parity );
paramStr = ( nameSpace + nodeName + "/rc_serial/Data_Bit");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, data_bit);
else
nh.setParam(paramStr, data_bit );
paramStr = ( nameSpace + nodeName + "/rc_serial/Stop_Bits");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, stop_bit);
else
nh.setParam(paramStr, stop_bit );
paramStr = ( nameSpace + nodeName + "/rc_serial/Simulation_Active");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, simulMode);
else
nh.setParam(paramStr, simulMode );
// <<<<< Serial Port
// >>>>> Robot Params to be saved on RoboController's EEPROM
paramStr = ( nameSpace + nodeName + "/robot_param/Weight_g");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.Weight);
else
nh.setParam(paramStr, 5000 );
paramStr = ( nameSpace + nodeName + "/robot_param/Width_mm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.Width);
else
nh.setParam(paramStr, 420 );
paramStr = ( nameSpace + nodeName + "/robot_param/Height_mm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.Height);
else
nh.setParam(paramStr, 200 );
paramStr = ( nameSpace + nodeName + "/robot_param/Lenght_mm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.Lenght);
else
nh.setParam(paramStr, 365 );
paramStr = ( nameSpace + nodeName + "/robot_param/WheelBase_mm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.WheelBase);
else
nh.setParam(paramStr, 340 );
paramStr = ( nameSpace + nodeName + "/robot_param/WheelRadiusLeft_cent_mm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.WheelRadiusLeft);
else
nh.setParam(paramStr, 3500 );
paramStr = ( nameSpace + nodeName + "/robot_param/WheelRadiusRight_cent_mm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.WheelRadiusRight);
else
nh.setParam(paramStr, 3500 );
paramStr = ( nameSpace + nodeName + "/robot_param/EncoderCprLeft");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.EncoderCprLeft);
else
nh.setParam(paramStr, 400 );
paramStr = ( nameSpace + nodeName + "/robot_param/EncoderCprRight");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.EncoderCprRight);
else
nh.setParam(paramStr, 400 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxRpmMotorLeft");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxRpmMotorLeft);
else
nh.setParam(paramStr, 218 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxRpmMotorRight");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxRpmMotorRight);
else
nh.setParam(paramStr, 218 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxAmpereMotorLeft_mA");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxAmpereMotorLeft);
else
nh.setParam(paramStr, 1650 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxAmpereMotorRight_mA");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxAmpereMotorRight);
else
nh.setParam(paramStr, 1650 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxTorqueMotorLeft_Ncm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxTorqueMotorLeft);
else
nh.setParam(paramStr, 60 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxTorqueMotorRight_Ncm");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxTorqueMotorRight);
else
nh.setParam(paramStr, 60 );
paramStr = ( nameSpace + nodeName + "/robot_param/RatioShaftLeft");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.RatioShaftLeft);
else
nh.setParam(paramStr, 3 );
paramStr = ( nameSpace + nodeName + "/robot_param/RatioShaftRight");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.RatioShaftRight);
else
nh.setParam(paramStr, 3 );
paramStr = ( nameSpace + nodeName + "/robot_param/RatioMotorLeft");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.RatioMotorLeft);
else
nh.setParam(paramStr, 55 );
paramStr = ( nameSpace + nodeName + "/robot_param/RatioMotorRight");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.RatioMotorRight);
else
nh.setParam(paramStr, 55 );
int val;
paramStr = ( nameSpace + nodeName + "/robot_param/MotorEnableLevel");
if(nh.hasParam( paramStr ))
{
nh.getParam(paramStr, val);
rbConf.MotorEnableLevel = static_cast<PinLevel>(val);
}
else
nh.setParam(paramStr, 1 );
paramStr = ( nameSpace + nodeName + "/robot_param/EncoderPosition");
if(nh.hasParam( paramStr ))
{
nh.getParam(paramStr, val/*rbConf.EncoderPosition*/);
rbConf.EncoderPosition = static_cast<EncoderPos>(val);
}
else
nh.setParam(paramStr, 0 );
paramStr = ( nameSpace + nodeName + "/robot_param/MaxChargedBatteryLevel_mV");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MaxChargedBatteryLevel);
else
nh.setParam(paramStr, 16800 );
paramStr = ( nameSpace + nodeName + "/robot_param/MinChargedBatteryLevel_mV");
if(nh.hasParam( paramStr ))
nh.getParam(paramStr, rbConf.MinChargedBatteryLevel);
else
nh.setParam(paramStr, 12000 );
// <<<<< Robot Params to be saved on RoboController's EEPROM
}
void imageCb(const ImageConstPtr& image_msg,
const DisparityImageConstPtr& disparity_msg,
const RoisConstPtr& rois_msg){
bool label_all;
vector<Rect> R_out;
vector<int> L_out;
string cfnm;
int numSamples;
string node_name = ros::this_node::getName();
//Use CV Bridge to convert images
CvImagePtr cv_gray = cv_bridge::toCvCopy(image_msg,image_encodings::MONO8);
Mat im_gray = cv_gray->image;
// take the region of interest message and create vectors of ROIs and labels
vector<int> L_in;
vector<Rect> R_in;
R_in.clear();
L_in.clear();
for(unsigned int i=0;i<rois_msg->rois.size();i++){
int x = rois_msg->rois[i].x;
int y = rois_msg->rois[i].y;
int w = rois_msg->rois[i].width;
int h = rois_msg->rois[i].height;
int l = rois_msg->rois[i].label;
if(x+w< im_gray.cols && y+h < im_gray.rows){
Rect R(x,y,w,h);
R_in.push_back(R);
L_in.push_back(l);
}
else{
ROS_ERROR("Roi out of bounds x=%d y=%d w=%d h=%d type=%d",x,y,w,h,l);
}
}
// get training prior
double temp;
if(!node_.getParam(node_name + "/HaarAdaPrior",temp)){
temp = 0.0;
}
HAC_.HaarAdaPrior_ = temp;
// do the work of the node
switch(get_mode()){
case DETECT:
label_all = false;
HAC_.detect(R_in,L_in,im_gray,R_out,L_out,label_all);
output_rois_.rois.clear();
output_rois_.header.stamp = image_msg->header.stamp;
output_rois_.header.frame_id = image_msg->header.frame_id;
ROS_INFO("HaarAda found %d objects",(int)L_out.size());
for(unsigned int i=0; i<R_out.size();i++){
RoiRect R;
R.x = R_out[i].x;
R.y = R_out[i].y;
R.width = R_out[i].width;
R.height = R_out[i].height;
R.label = L_out[i];
output_rois_.rois.push_back(R);
}
if(!node_.getParam(node_name + "/RemoveOverlappingRois",remove_overlapping_rois_)){
remove_overlapping_rois_ = false;
}
if(remove_overlapping_rois_){
// ROS_ERROR("removing overlapping rois");
remove_overlap_Rois(output_rois_, non_overlapping_rois_);
non_overlapping_rois_.header.stamp = image_msg->header.stamp;
non_overlapping_rois_.header.frame_id = image_msg->header.frame_id;
pub_rois_.publish(non_overlapping_rois_);
}
else{
pub_rois_.publish(output_rois_);
}
ROS_INFO("publishing image");
pub_Color_Image_.publish(image_msg);
pub_Disparity_Image_.publish(disparity_msg);
break;
case ACCUMULATE:
numSamples = HAC_.addToTraining(R_in,L_in,im_gray);
int NS;
if(node_.getParam(node_name + "/num_Training_Samples",NS)){
float percent = (float)HAC_.numSamples_ * 100.0/NS;
ROS_INFO("ACCUMULATING: %6.1f%c",percent,'%');
if(numSamples >= NS){
node_.setParam(node_name + "/mode", std::string("train"));
ROS_ERROR("DONE Accumulating, switching to train mode");
}
}
break;
case TRAIN:
node_.param(node_name + "/classifier_file",cfnm,std::string("/home/clewis/classifiers/test.xml"));
ROS_ERROR("Submitting %d Samples to Train ouput= %s",HAC_.numSamples_,cfnm.c_str());
HAC_.train(cfnm);
node_.setParam(node_name + "/mode", std::string("evaluate"));
ROS_ERROR("DONE TRAINING, switching to evaluate mode");
break;
case EVALUATE:
{
if(!HAC_.loaded){
node_.param(node_name + "/classifier_file",cfnm,std::string("test.xml"));
ROS_ERROR("HaarAda LOADING %s",cfnm.c_str());
HAC_.load(cfnm);
}
label_all = false;
HAC_.detect(R_in,L_in,im_gray,R_out,L_out,label_all);
int total0_in_list=0;
int total1_in_list=0;
int fp_in_list=0;
int tp_in_list=0;
// count the input labels
for(unsigned int i=0; i<R_in.size();i++){
if(L_in[i] == 0 || L_in[i] == -1) total0_in_list++;
if(L_in[i] == 1) total1_in_list++;
}
num_class0 += total0_in_list;
num_class1 += total1_in_list;
// count the output labels which have the correct and incorrect label
for(unsigned int i=0; i<R_out.size();i++){
if(L_out[i] == 1){
tp_in_list++;
}
else fp_in_list++;
}
num_TP_class1 += tp_in_list;
num_FP_class1 += fp_in_list;
num_TP_class0 += total0_in_list - fp_in_list;
num_FP_class0 += total1_in_list - tp_in_list;
float tp1 = (float)num_TP_class1 / (float) num_class1 * 100.0;
float tp0 = (float)num_TP_class0 / (float) num_class0 * 100.0;
float fp1 = (float)num_FP_class1 / (float) num_class1 * 100.0;
float fp0 = (float)num_FP_class0 / (float) num_class0 * 100.0;
ROS_ERROR("TP0 = %6.2f FP0 = %6.2f TP1 = %6.2f FP1 = %6.2f",tp0,fp0,tp1,fp1);
}
break;
case LOAD:
node_.param(node_name + "/classifier_file",cfnm,std::string("test.xml"));
ROS_ERROR("HaarAda LOADING %s",cfnm.c_str());
HAC_.load(cfnm);
node_.setParam(node_name+"/mode", std::string("detect"));
break;
}// end switch
}
void
AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
last_laser_received_ts_ = ros::Time::now();
if( map_ == NULL ) {
return;
}
boost::recursive_mutex::scoped_lock lr(configuration_mutex_);
int laser_index = -1;
// Do we have the base->base_laser Tx yet?
if(frame_to_laser_.find(laser_scan->header.frame_id) == frame_to_laser_.end())
{
ROS_DEBUG("Setting up laser %d (frame_id=%s)\n", (int)frame_to_laser_.size(), laser_scan->header.frame_id.c_str());
lasers_.push_back(new AMCLLaser(*laser_));
lasers_update_.push_back(true);
laser_index = frame_to_laser_.size();
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)),
ros::Time(), laser_scan->header.frame_id);
tf::Stamped<tf::Pose> laser_pose;
try
{
this->tf_->transformPose(base_frame_id_, ident, laser_pose);
}
catch(tf::TransformException& e)
{
ROS_ERROR("Couldn't transform from %s to %s, "
"even though the message notifier is in use",
laser_scan->header.frame_id.c_str(),
base_frame_id_.c_str());
return;
}
pf_vector_t laser_pose_v;
laser_pose_v.v[0] = laser_pose.getOrigin().x();
laser_pose_v.v[1] = laser_pose.getOrigin().y();
// laser mounting angle gets computed later -> set to 0 here!
laser_pose_v.v[2] = 0;
lasers_[laser_index]->SetLaserPose(laser_pose_v);
ROS_DEBUG("Received laser's pose wrt robot: %.3f %.3f %.3f",
laser_pose_v.v[0],
laser_pose_v.v[1],
laser_pose_v.v[2]);
frame_to_laser_[laser_scan->header.frame_id] = laser_index;
} else {
// we have the laser pose, retrieve laser index
laser_index = frame_to_laser_[laser_scan->header.frame_id];
}
// Where was the robot when this scan was taken?
tf::Stamped<tf::Pose> odom_pose;
pf_vector_t pose;
if(!getOdomPose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
laser_scan->header.stamp, base_frame_id_))
{
ROS_ERROR("Couldn't determine robot's pose associated with laser scan");
return;
}
pf_vector_t delta = pf_vector_zero();
if(pf_init_)
{
// Compute change in pose
//delta = pf_vector_coord_sub(pose, pf_odom_pose_);
delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
// See if we should update the filter
bool update = fabs(delta.v[0]) > d_thresh_ ||
fabs(delta.v[1]) > d_thresh_ ||
fabs(delta.v[2]) > a_thresh_;
update = update || m_force_update;
m_force_update=false;
// Set the laser update flags
if(update)
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
}
bool force_publication = false;
if(!pf_init_)
{
// Pose at last filter update
pf_odom_pose_ = pose;
// Filter is now initialized
pf_init_ = true;
// Should update sensor data
for(unsigned int i=0; i < lasers_update_.size(); i++)
lasers_update_[i] = true;
force_publication = true;
resample_count_ = 0;
}
// If the robot has moved, update the filter
else if(pf_init_ && lasers_update_[laser_index])
{
//printf("pose\n");
//pf_vector_fprintf(pose, stdout, "%.3f");
AMCLOdomData odata;
odata.pose = pose;
// HACK
// Modify the delta in the action data so the filter gets
// updated correctly
odata.delta = delta;
// Use the action data to update the filter
odom_->UpdateAction(pf_, (AMCLSensorData*)&odata);
// Pose at last filter update
//this->pf_odom_pose = pose;
}
bool resampled = false;
// If the robot has moved, update the filter
if(lasers_update_[laser_index])
{
AMCLLaserData ldata;
ldata.sensor = lasers_[laser_index];
ldata.range_count = laser_scan->ranges.size();
// To account for lasers that are mounted upside-down, we determine the
// min, max, and increment angles of the laser in the base frame.
//
// Construct min and max angles of laser, in the base_link frame.
tf::Quaternion q;
q.setRPY(0.0, 0.0, laser_scan->angle_min);
tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,
laser_scan->header.frame_id);
try
{
tf_->transformQuaternion(base_frame_id_, min_q, min_q);
tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);
}
catch(tf::TransformException& e)
{
ROS_WARN("Unable to transform min/max laser angles into base frame: %s",
e.what());
return;
}
double angle_min = tf::getYaw(min_q);
double angle_increment = tf::getYaw(inc_q) - angle_min;
// wrapping angle to [-pi .. pi]
angle_increment = fmod(angle_increment + 5*M_PI, 2*M_PI) - M_PI;
ROS_DEBUG("Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment);
// Apply range min/max thresholds, if the user supplied them
if(laser_max_range_ > 0.0)
ldata.range_max = std::min(laser_scan->range_max, (float)laser_max_range_);
else
ldata.range_max = laser_scan->range_max;
double range_min;
if(laser_min_range_ > 0.0)
range_min = std::max(laser_scan->range_min, (float)laser_min_range_);
else
range_min = laser_scan->range_min;
// The AMCLLaserData destructor will free this memory
ldata.ranges = new double[ldata.range_count][2];
ROS_ASSERT(ldata.ranges);
for(int i=0; i<ldata.range_count; i++)
{
// amcl doesn't (yet) have a concept of min range. So we'll map short
// readings to max range.
if(laser_scan->ranges[i] <= range_min)
ldata.ranges[i][0] = ldata.range_max;
else
ldata.ranges[i][0] = laser_scan->ranges[i];
// Compute bearing
ldata.ranges[i][1] = angle_min +
(i * angle_increment);
}
lasers_[laser_index]->UpdateSensor(pf_, (AMCLSensorData*)&ldata);
lasers_update_[laser_index] = false;
pf_odom_pose_ = pose;
// Resample the particles
if(!(++resample_count_ % resample_interval_))
{
pf_update_resample(pf_);
resampled = true;
}
pf_sample_set_t* set = pf_->sets + pf_->current_set;
ROS_DEBUG("Num samples: %d\n", set->sample_count);
// Publish the resulting cloud
// TODO: set maximum rate for publishing
if (!m_force_update) {
geometry_msgs::PoseArray cloud_msg;
cloud_msg.header.stamp = ros::Time::now();
cloud_msg.header.frame_id = global_frame_id_;
cloud_msg.poses.resize(set->sample_count);
for(int i=0; i<set->sample_count; i++)
{
tf::poseTFToMsg(tf::Pose(tf::createQuaternionFromYaw(set->samples[i].pose.v[2]),
tf::Vector3(set->samples[i].pose.v[0],
set->samples[i].pose.v[1], 0)),
cloud_msg.poses[i]);
}
particlecloud_pub_.publish(cloud_msg);
}
}
if(resampled || force_publication)
{
// Read out the current hypotheses
double max_weight = 0.0;
int max_weight_hyp = -1;
std::vector<amcl_hyp_t> hyps;
hyps.resize(pf_->sets[pf_->current_set].cluster_count);
for(int hyp_count = 0;
hyp_count < pf_->sets[pf_->current_set].cluster_count; hyp_count++)
{
double weight;
pf_vector_t pose_mean;
pf_matrix_t pose_cov;
if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean, &pose_cov))
{
ROS_ERROR("Couldn't get stats on cluster %d", hyp_count);
break;
}
hyps[hyp_count].weight = weight;
hyps[hyp_count].pf_pose_mean = pose_mean;
hyps[hyp_count].pf_pose_cov = pose_cov;
if(hyps[hyp_count].weight > max_weight)
{
max_weight = hyps[hyp_count].weight;
max_weight_hyp = hyp_count;
}
}
if(max_weight > 0.0)
{
ROS_DEBUG("Max weight pose: %.3f %.3f %.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
/*
puts("");
pf_matrix_fprintf(hyps[max_weight_hyp].pf_pose_cov, stdout, "%6.3f");
puts("");
*/
geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
p.pose.pose.orientation);
// Copy in the covariance, converting from 3-D to 6-D
pf_sample_set_t* set = pf_->sets + pf_->current_set;
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j];
p.pose.covariance[6*i+j] = set->cov.m[i][j];
}
}
// Report the overall filter covariance, rather than the
// covariance for the highest-weight cluster
//p.covariance[6*5+5] = hyps[max_weight_hyp].pf_pose_cov.m[2][2];
p.pose.covariance[6*5+5] = set->cov.m[2][2];
/*
printf("cov:\n");
for(int i=0; i<6; i++)
{
for(int j=0; j<6; j++)
printf("%6.3f ", p.covariance[6*i+j]);
puts("");
}
*/
pose_pub_.publish(p);
last_published_pose = p;
ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);
// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}
latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
}
else
{
ROS_ERROR("No pose!");
}
}
else if(latest_tf_valid_)
{
if (tf_broadcast_ == true)
{
// Nothing changed, so we'll just republish the last transform, to keep
// everybody happy.
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
}
// Is it time to save our last pose to the param server
ros::Time now = ros::Time::now();
if((save_pose_period.toSec() > 0.0) &&
(now - save_pose_last_time) >= save_pose_period)
{
// We need to apply the last transform to the latest odom pose to get
// the latest map pose to store. We'll take the covariance from
// last_published_pose.
tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
double yaw,pitch,roll;
map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());
private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());
private_nh_.setParam("initial_pose_a", yaw);
private_nh_.setParam("initial_cov_xx",
last_published_pose.pose.covariance[6*0+0]);
private_nh_.setParam("initial_cov_yy",
last_published_pose.pose.covariance[6*1+1]);
private_nh_.setParam("initial_cov_aa",
last_published_pose.pose.covariance[6*5+5]);
save_pose_last_time = now;
}
}
}
// load robot footprint from costmap_2d_ros to keep same footprint format
std::vector<geometry_msgs::Point> CollisionVelocityFilter::loadRobotFootprint(ros::NodeHandle node){
std::vector<geometry_msgs::Point> footprint;
geometry_msgs::Point pt;
double padding;
std::string padding_param, footprint_param;
if(!node.searchParam("footprint_padding", padding_param))
padding = 0.01;
else
node.param(padding_param, padding, 0.01);
//grab the footprint from the parameter server if possible
XmlRpc::XmlRpcValue footprint_list;
std::string footprint_string;
std::vector<std::string> footstring_list;
if(node.searchParam("footprint", footprint_param)){
node.getParam(footprint_param, footprint_list);
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
footprint_string = std::string(footprint_list);
//if there's just an empty footprint up there, return
if(footprint_string == "[]" || footprint_string == "")
return footprint;
boost::erase_all(footprint_string, " ");
boost::char_separator<char> sep("[]");
boost::tokenizer<boost::char_separator<char> > tokens(footprint_string, sep);
footstring_list = std::vector<std::string>(tokens.begin(), tokens.end());
}
//make sure we have a list of lists
if(!(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_list.size() > 2) && !(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString && footstring_list.size() > 5)){
ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", footprint_param.c_str(), std::string(footprint_list).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]]");
}
if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(int i = 0; i < footprint_list.size(); ++i){
//make sure we have a list of lists of size 2
XmlRpc::XmlRpcValue point = footprint_list[i];
if(!(point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2)){
ROS_FATAL("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");
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");
}
//make sure that the value we're looking at is either a double or an int
if(!(point[0].getType() == XmlRpc::XmlRpcValue::TypeInt || point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.x = point[0].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[0]) : (double)(point[0]);
pt.x += sign(pt.x) * padding;
//make sure that the value we're looking at is either a double or an int
if(!(point[1].getType() == XmlRpc::XmlRpcValue::TypeInt || point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble)){
ROS_FATAL("Values in the footprint specification must be numbers");
throw std::runtime_error("Values in the footprint specification must be numbers");
}
pt.y = point[1].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(point[1]) : (double)(point[1]);
pt.y += sign(pt.y) * padding;
footprint.push_back(pt);
node.deleteParam(footprint_param);
std::ostringstream oss;
bool first = true;
BOOST_FOREACH(geometry_msgs::Point p, footprint) {
if(first) {
oss << "[[" << p.x << "," << p.y << "]";
first = false;
}
else {
oss << ",[" << p.x << "," << p.y << "]";
}
}
oss << "]";
node.setParam(footprint_param, oss.str().c_str());
node.setParam("footprint", oss.str().c_str());
}
}
else if(footprint_list.getType() == XmlRpc::XmlRpcValue::TypeString) {
// Subscribe to models of interest. Currently, we find and subscribe
// to the first 'laser' model and the first 'position' model. Returns
// 0 on success (both models subscribed), -1 otherwise.
//
// Eventually, we should provide a general way to map stage models onto ROS
// topics, similar to Player .cfg files.
int
StageNode::SubscribeModels()
{
n_.setParam("/use_sim_time", true);
for (size_t r = 0; r < this->positionmodels.size(); r++)
{
StageRobot* new_robot = new StageRobot;
new_robot->positionmodel = this->positionmodels[r];
new_robot->positionmodel->Subscribe();
for (size_t s = 0; s < this->lasermodels.size(); s++)
{
if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel)
{
new_robot->lasermodels.push_back(this->lasermodels[s]);
this->lasermodels[s]->Subscribe();
}
}
for (size_t s = 0; s < this->cameramodels.size(); s++)
{
if (this->cameramodels[s] and this->cameramodels[s]->Parent() == new_robot->positionmodel)
{
new_robot->cameramodels.push_back(this->cameramodels[s]);
this->cameramodels[s]->Subscribe();
}
}
ROS_INFO("Found %lu laser devices and %lu cameras in robot %lu", new_robot->lasermodels.size(), new_robot->cameramodels.size(), r);
new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
new_robot->cmdvel_sub = n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));
for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
{
if (new_robot->lasermodels.size() == 1)
new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(this->laser_topic.c_str(), r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
else
new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(this->laser_topic.c_str(), r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
}
for (size_t s = 0; s < new_robot->cameramodels.size(); ++s)
{
if (new_robot->cameramodels.size() == 1)
{
new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
}
else
{
new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
}
}
this->robotmodels_.push_back(new_robot);
}
clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock", 10);
// advertising reset service
reset_srv_ = n_.advertiseService("reset_positions", &StageNode::cb_reset_srv, this);
guirequest_sub = n_.subscribe<std_msgs::String>("/stageGUIRequest",10,boost::bind(&StageNode::GUIRequest_cb, this, _1));
// n_.subscribe("/stageGUIRequest", 10, &StageNode::GUIRequest_cb, this);
return(0);
}
void Costmap2DROS::resetOldParameters(ros::NodeHandle& nh)
{
ROS_INFO("Loading from pre-hydro parameter style");
bool flag;
std::string s;
std::vector < XmlRpc::XmlRpcValue > plugins;
XmlRpc::XmlRpcValue::ValueStruct map;
SuperValue super_map;
SuperValue super_array;
if (nh.getParam("static_map", flag) && flag)
{
map["name"] = XmlRpc::XmlRpcValue("static_layer");
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
super_map.setStruct(&map);
plugins.push_back(super_map);
ros::NodeHandle map_layer(nh, "static_layer");
move_parameter(nh, map_layer, "map_topic");
move_parameter(nh, map_layer, "unknown_cost_value");
move_parameter(nh, map_layer, "lethal_cost_threshold");
move_parameter(nh, map_layer, "track_unknown_space", false);
}
ros::NodeHandle obstacles(nh, "obstacle_layer");
if (nh.getParam("map_type", s) && s == "voxel")
{
map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
super_map.setStruct(&map);
plugins.push_back(super_map);
move_parameter(nh, obstacles, "origin_z");
move_parameter(nh, obstacles, "z_resolution");
move_parameter(nh, obstacles, "z_voxels");
move_parameter(nh, obstacles, "mark_threshold");
move_parameter(nh, obstacles, "unknown_threshold");
move_parameter(nh, obstacles, "publish_voxel_map");
}
else
{
map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
super_map.setStruct(&map);
plugins.push_back(super_map);
}
move_parameter(nh, obstacles, "max_obstacle_height");
move_parameter(nh, obstacles, "raytrace_range");
move_parameter(nh, obstacles, "obstacle_range");
move_parameter(nh, obstacles, "track_unknown_space", true);
nh.param("observation_sources", s, std::string(""));
std::stringstream ss(s);
std::string source;
while (ss >> source)
{
move_parameter(nh, obstacles, source);
}
move_parameter(nh, obstacles, "observation_sources");
ros::NodeHandle inflation(nh, "inflation_layer");
move_parameter(nh, inflation, "cost_scaling_factor");
move_parameter(nh, inflation, "inflation_radius");
map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
super_map.setStruct(&map);
plugins.push_back(super_map);
super_array.setArray(&plugins);
nh.setParam("plugins", super_array);
}