Ejemplo n.º 1
0
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();
	}
}
Ejemplo n.º 2
0
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();
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
 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;
  );
Ejemplo n.º 6
0
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");
    }
}
Ejemplo n.º 7
0
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);

}
Ejemplo n.º 10
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);


}