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); }