void
Robot::draw()
{
tf::Transform robot_pose, opengl_to_robot_pose;
robot_pose.setOrigin(tf::Vector3(getRobotPose().position.x, getRobotPose().position.y, getRobotPose().position.z));
robot_pose.setRotation(tf::Quaternion(getRobotPose().orientation.yaw, getRobotPose().orientation.pitch, getRobotPose().orientation.roll));
opengl_to_robot_pose.setOrigin(tf::Vector3(1.4, 0.0, 0.0));
opengl_to_robot_pose.setRotation(tf::Quaternion(0.0, 0.0, 0.0));
robot_pose = robot_pose * opengl_to_robot_pose;
if(this->robot_model_ != NULL)
{
glPushMatrix();
glTranslatef(robot_pose.getOrigin().x(),robot_pose.getOrigin().y(), robot_pose.getOrigin().z() + 0.28);
double y, p, r;
tf::Matrix3x3(robot_pose.getRotation()).getRPY(r, p, y);
glRotatef(carmen_radians_to_degrees(y), 0.0f, 0.0f, 1.0f);
glRotatef(carmen_radians_to_degrees(p), 0.0f, 1.0f, 0.0f);
glRotatef(carmen_radians_to_degrees(r), 1.0f, 0.0f, 0.0f);
glRotatef(90.0, 1.0, 0.0, 0.0);
glRotatef(0.0, 0.0, 1.0, 0.0);
glmDraw(this->robot_model_, GLM_SMOOTH | GLM_COLOR);
glPopMatrix();
}
}
void
Robot::drawEllipse()
{
glColor3f(0.6, 0.34, 0.8);
tf::Transform robot_pose, opengl_to_robot_pose;
robot_pose.setOrigin(tf::Vector3(getRobotPose().position.x, getRobotPose().position.y, getRobotPose().position.z));
robot_pose.setRotation(tf::Quaternion(getRobotPose().orientation.yaw, getRobotPose().orientation.pitch, getRobotPose().orientation.roll));
glPushMatrix();
glTranslatef(robot_pose.getOrigin().x(),robot_pose.getOrigin().y(), robot_pose.getOrigin().z());
glRotatef(this->angle_, 0.0f, 0.0f, 1.0f);
glBegin(GL_LINE_LOOP);
for(int i=0; i < 360; i++)
{
//convert degrees into radians
float degInRad = carmen_degrees_to_radians((double)i);
glVertex2f(cos(degInRad)*this->minor_axis_,sin(degInRad)*this->major_axis_);
}
glEnd();
glPopMatrix();
}
BKPlanner::BKPlanner(std::string name, tf::TransformListener& tf):
nh_ (),
priv_nh_ ("~"),
tf_ (tf),
got_new_goal_ (false),
last_accepted_goal_ (ros::Time(0)),
sc_ ()
{
// Initialize the cost map and path checker
planner_costmap_ = shared_ptr<costmap_2d::Costmap2DROS>
(new costmap_2d::Costmap2DROS("local_costmap", tf_) );
path_checker_ = shared_ptr<path_checker::PathChecker>
(new path_checker::PathChecker("path_checker", planner_costmap_));
// Get planner-related parameters
priv_nh_.param("goal_hysteresis", goal_hysteresis_, 0.2);
ROS_INFO("[bk_planner] Goal hysteresis is %.2f", goal_hysteresis_);
double temp;
priv_nh_.param("goal_timeout", temp, 3.0);
goal_timeout_ = ros::Duration(temp);
ROS_INFO("[bk_planner] Goal timeout is %.2fsec", temp);
priv_nh_.param("goal_cov_thresh", goal_cov_thresh_, 1.0);
ROS_INFO("[bk_planner] Goal covariance threshold is %.2f", goal_cov_thresh_);
// Get the robot's current pose and set a goal there
PoseStamped robot_pose;
if( getRobotPose(robot_pose) ) {
setNewGoal(robot_pose);
}
else {
ROS_ERROR("[bk_planner] Could not transform start pose");
}
// Create the planner and feeder threads
planner_ = shared_ptr<BKPlanningThread>
(new BKPlanningThread(this) );
feeder_ = shared_ptr<BKFeederThread>
(new BKFeederThread (this) );
// Set their references to each other
planner_->setFeeder (feeder_);
feeder_ ->setPlanner(planner_);
// Kick off the threads
planning_thread_ = shared_ptr<boost::thread>
(new boost::thread(boost::bind(&BKPlanningThread::run, planner_)) );
feeder_thread_ = shared_ptr<boost::thread>
(new boost::thread(boost::bind(&BKFeederThread::run , feeder_ )) );
// This node subscribes to a goal pose.
goal_sub_ = nh_.subscribe("goal_with_covariance", 1, &BKPlanner::goalCB, this);
// ROS_INFO("BKPlanner constructor finished");
return;
}
bool BallPickerLayer::clearObstacles(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
clear_flag = true;
tf::Stamped <tf::Pose> pose;
if (getRobotPose(pose))
layered_costmap_->updateMap(pose.getOrigin().x(), pose.getOrigin().y(), tf::getYaw(pose.getRotation()));
else
layered_costmap_->updateMap(0.0, 0.0, 0.0);
clear_flag = false;
}
void GPS_SelfLocator::execute()
{
Pose2D tmpPose = calculateFromGPS(getGPSData());
Pose2D gpsOdometryDelta = tmpPose - gpsRobotPose;
gpsRobotPose = tmpPose;
Pose2D odometryDelta = lastRobotOdometry - getOdometryData();
lastRobotOdometry = getOdometryData();
DEBUG_REQUEST("GPS_SelfLocator:use_GPSData",
getRobotPose() = gpsRobotPose;
);
void JustinaKnowledge::addUpdateKnownLoc(std::string name){
knowledge_msgs::AddUpdateKnownLoc srv;
std::vector<float> values;
float x, y, theta;
getRobotPose(x, y, theta);
srv.request.loc.name = name;
values.push_back(x);
values.push_back(y);
srv.request.loc.value = values;
if (cliAddUpKnownLoc->call(srv)) {
} else {
ROS_ERROR("Failed to call service known_locations");
}
}
void pointCloudCallback(const sensor_msgs::PointCloud2& traversability_msg) {
//pcl::PointCloud<pcl::PointXYZI> traversability_pcl;
pcl::fromROSMsg(traversability_msg, traversability_pcl);
ROS_INFO("path planner input set");
if (traversability_pcl.size() > 0 && wall_flag){
tf::StampedTransform robot_pose;
getRobotPose(robot_pose);
pcl::PointXYZI robot;
robot.x = robot_pose.getOrigin().x();
robot.y = robot_pose.getOrigin().y();
robot.z = robot_pose.getOrigin().z();
//pcl::KdTreeFLANN<pcl::PointXYZI> traversability_kdtree;
traversability_kdtree.setInputCloud(traversability_pcl.makeShared());
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
traversability_kdtree.nearestKSearch(robot, 1, pointIdxNKNSearch, pointNKNSquaredDistance);
robot_idx = pointIdxNKNSearch[0];
//----------------------------------------------------------------------------------------------------------------
//ho commentato questa parte perchè vogliamo disaccoppiare il planning dall'acquisizione della Point Cloud
//----------------------------------------------------------------------------------------------------------------
// planner->set_input(traversability_pcl, wall_pcl, wall_kdTree, traversability_kdtree, pointIdxNKNSearch[0]);
// wall_flag = false;
// if (goal_selection){
// nav_msgs::Path path;
// ROS_INFO("compute path");
// if(goal_selection){
// if(planner->planning(path)){
// path_pub.publish(path);
// }
// else{
// ROS_INFO("no path exist for desired goal, please choose another goal");
// goal_selection = true;
// }
// ROS_INFO("path_computed");
// }
// }
}
}
InputOutputLinControl::InputOutputLinControl():
displacement(0.2),
plan_received(false),
vel_g(0.15)
{
global_path_ = node.advertise<nav_msgs::Path>("global_path",1);
local_path_ = node.advertise<nav_msgs::Path>("local_path",1);
robot_pose_marker_pub = node.advertise<visualization_msgs::Marker>("robot_pose",1);
true_robot_pose_marker_pub = node.advertise<visualization_msgs::Marker>("true_robot_pose",1);
icp_robot_pose_marker_pub = node.advertise<visualization_msgs::Marker>("/icp_robot_pose",1);
imu_odom_sub = node.subscribe("/imu_odom",1,&InputOutputLinControl::imuOdomCallback,this);
//icp_odom_sub = node.subscribe("/icp_odom",1,&InputOutputLinControl::icpOdomCallback,this);
//global_plan_sub = node.subscribe("/final_stigmergy_path",1,&InputOutputLinControl::globalPathCallback,this);
global_plan_sub = node.subscribe("/final_stigmergy_path",1,&InputOutputLinControl::globalPathCallback2,this);
trajectory_error_pub = node.advertise<input_output_lin_control::TrajectoryError>("/trajectory_error",1);
robot_commands_pub = node.advertise<input_output_lin_control::RobotCommands>("/robot_commands",1);
global_path.header.frame_id = "/map";
global_path.header.stamp = ros::Time::now();
local_path.header.frame_id = "/map";
local_path.header.stamp = ros::Time::now();
while(!getRobotPose(real_robot_pose_odom,real_robot_pose_map,from_odom_to_map))
{
ROS_INFO("Waiting for transformation");
}
markerFromPose("true_robot_pose_marker",0.98,0.2,0.82,real_robot_pose_map,true_robot_pose_marker);
true_robot_pose_marker_pub.publish(true_robot_pose_marker);
realRobotPoseB(displacement,real_robot_pose_map,real_robot_poseB_map);
markerFromPose("robot_poseB_marker",0,1,0,real_robot_poseB_map,robot_pose_marker);
robot_pose_marker_pub.publish(robot_pose_marker);
//markerFromPose("icp_robot_pose",1,1,1,real_robot_pose_map,icp_robot_pose_marker);
//icp_robot_pose_marker_pub.publish(icp_robot_pose_marker);
realRobotPoseB(displacement,real_robot_pose_odom,real_robot_poseB_odom);
}
StigmergyPlanner::StigmergyPlanner():starting_pose_ready(false),drawn_path(false),finished(false)
{
path_pub = node.advertise<nav_msgs::Path>("/stigmergy_path",1);
final_path_pub = node.advertise<nav_msgs::Path>("/final_stigmergy_path",1);
target_pub = node.advertise<visualization_msgs::Marker>("/target",1);
server.reset( new interactive_markers::InteractiveMarkerServer("stigmergy_planner","",false) );
menu_handler.insert("Select Starting Pose",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Undo Starting Pose",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Apply Plan",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Discard Plan",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Undo Planning",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.insert("Evaluate Plan",boost::bind(&StigmergyPlanner::processFeedback,this,_1));
while(!getRobotPose(robot_pose))
{
ROS_INFO("Waiting for transformation");
}
makeViewFacingMarker(robot_pose);
server->applyChanges();
target.header.frame_id = "/map";
target.header.stamp = ros::Time::now();
target.id = 1;
target.ns = "target";
target.scale.x = 0.3;
target.scale.y = 0.3;
target.scale.z = 0.3;
target.type = visualization_msgs::Marker::MESH_RESOURCE;
target.mesh_resource = "package://stigmergy_planner/meshes/barrier.dae";
target.mesh_use_embedded_materials = true;
target.lifetime = ros::Duration(0);
}
void CloudAssembler::process(const sensor_msgs::PointCloud2::ConstPtr &cloud)
{
ROS_INFO_STREAM_ONCE("CloudAssembler::process(): Point cloud received");
geometry_msgs::PoseStamped p;
if (!getRobotPose(cloud->header.stamp, p)) return;
bool update = false;
double dist = sqrt(pow(robot_pose_.pose.position.x - p.pose.position.x, 2) + pow(robot_pose_.pose.position.y - p.pose.position.y, 2));
if (dist > dist_th_)
{
robot_pose_ = p;
if (dist > max_dist_th_)
{
cloud_buff_->clear();
}
else update = true;
}
VPointCloud vpcl;
TPointCloudPtr tpcl(new TPointCloud());
// Retrieve the input point cloud
pcl::fromROSMsg(*cloud, vpcl);
pcl::copyPointCloud(vpcl, *tpcl);
pcl::PassThrough< TPoint > pass;
pass.setInputCloud(tpcl);
pass.setFilterFieldName("x");
pass.setFilterLimits(min_x_, max_x_);
pass.filter(*tpcl);
pass.setInputCloud(tpcl);
pass.setFilterFieldName("y");
pass.setFilterLimits(min_y_, max_y_);
pass.filter(*tpcl);
pass.setInputCloud(tpcl);
pass.setFilterFieldName("z");
pass.setFilterLimits(min_z_, max_z_);
pass.filter(*tpcl);
pcl::ApproximateVoxelGrid<TPoint> psor;
psor.setInputCloud(tpcl);
psor.setDownsampleAllData(false);
psor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
psor.filter(*tpcl);
pcl::StatisticalOutlierRemoval< TPoint > foutl;
foutl.setInputCloud(tpcl);
foutl.setMeanK(filter_cloud_k_);
foutl.setStddevMulThresh(filter_cloud_th_);
foutl.filter(*tpcl);
pcl_ros::transformPointCloud("odom", *tpcl, *tpcl, listener_);
// get accumulated cloud
TPointCloudPtr pcl_out(new TPointCloud());
for (unsigned int i = 0; i < cloud_buff_->size(); i++)
{
*pcl_out += cloud_buff_->at(i);
}
// registration
if (cloud_buff_->size() > 0)
{
pcl::IterativeClosestPoint< TPoint, TPoint> icp;
icp.setInputCloud(tpcl);
icp.setInputTarget(pcl_out);
pcl::PointCloud<TPoint> aligned;
icp.align(aligned);
if (icp.hasConverged())
{
*tpcl = aligned;
std::cout << "ICP score: " << icp.getFitnessScore() << std::endl;
}
}
if (update) cloud_buff_->push_back(*tpcl);
if (points_pub_.getNumSubscribers() == 0)
{
return;
}
*pcl_out += *tpcl;
pcl::ApproximateVoxelGrid<TPoint> sor;
sor.setInputCloud(pcl_out);
sor.setDownsampleAllData(false);
sor.setLeafSize(filter_cloud_res_, filter_cloud_res_, filter_cloud_res_);
TPointCloudPtr pcl_filt(new TPointCloud());
sor.filter(*pcl_filt);
sensor_msgs::PointCloud2::Ptr cloud_out(new sensor_msgs::PointCloud2());
pcl::toROSMsg(*pcl_filt, *cloud_out);
//std::cout << "points: " << pcl_out->points.size() << std::endl;
cloud_out->header.stamp = cloud->header.stamp;
cloud_out->header.frame_id = fixed_frame_;
points_pub_.publish(cloud_out);
}
