void PotentialField::target_waypoint_callback( visualization_msgs::Marker::ConstPtr target_point_msgs) { static TargetWaypointFieldParamater param; double ver_x_p(param.ver_x_p); double ver_y_p(param.ver_y_p); ros::Time time = ros::Time::now(); geometry_msgs::PointStamped in, out; in.header = target_point_msgs->header; in.point = target_point_msgs->pose.position; tf::TransformListener tflistener; try { ros::Time now = ros::Time(0); tflistener.waitForTransform("/map", "/potential_field_link", now, ros::Duration(10.0)); tflistener.transformPoint("/potential_field_link", in.header.stamp, in, "/map", out); } catch (tf::TransformException &ex) { ROS_ERROR("%s", ex.what()); } for (GridMapIterator it(map_); !it.isPastEnd(); ++it) { Position position; map_.getPosition(*it, position); map_.at("target_waypoint_field", *it) = 0.0; map_.at("target_waypoint_field", *it) -= 0.5 * std::exp((-1.0 * (std::pow((position.y() - (out.point.y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((position.x() - (out.point.x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))); } map_.setTimestamp(time.toNSec()); }
void populateMap(GridMap &map, string layer,string file_path, float scale,float res) { cv_bridge::CvImage cv_image; Mat img = imread(file_path,CV_LOAD_IMAGE_GRAYSCALE); cv::Size img_size= img.size(); cv::resize(img,img,cv::Size(),scale,scale, CV_INTER_CUBIC); map.setGeometry(Length(img.rows*res,img.cols*res),res); for (GridMapIterator it(map); !it.isPastEnd(); ++it) { Position position; map.getPosition(*it, position); int x = (img.rows/2) + position.x()/res+res/2.0; int y = (img.cols/2) + position.y()/res+res/2.0; map.at(layer, *it) = img.at<uchar>(x,y)<200?1:0; } map.setTimestamp(ros::Time::now().toNSec()); ROS_INFO("Created map with size %f x %f m (%i x %i cells).",map.getLength().x(), map.getLength().y(),map.getSize()(0), map.getSize()(1)); }
void PotentialField::vscan_points_callback( sensor_msgs::PointCloud2::ConstPtr vscan_msg) { static VscanPointsFieldParamater param; double around_x(param.around_x); double around_y(param.around_y); ros::Time time = ros::Time::now(); pcl::PointCloud<pcl::PointXYZ> pcl_vscan; pcl::fromROSMsg(*vscan_msg, pcl_vscan); pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_vscan_ptr( new pcl::PointCloud<pcl::PointXYZ>(pcl_vscan)); double length_x = map_.getLength().x() / 2.0; double length_y = map_.getLength().y() / 2.0; for (GridMapIterator it(map_); !it.isPastEnd(); ++it) { Position position; map_.getPosition(*it, position); map_.at("vscan_points_field", *it) = 0.0; for (int i(0); i < (int)pcl_vscan.size(); ++i) { double point_x = pcl_vscan.at(i).x - map_x_offset_; if (3.0 < pcl_vscan.at(i).z + tf_z_ || pcl_vscan.at(i).z + tf_z_ < 0.3) continue; if (length_x < point_x && point_x < -1.0 * length_x) continue; if (length_y < pcl_vscan.at(i).y && pcl_vscan.at(i).y < -1.0 * length_y) continue; if ((point_x + tf_x_) - around_x < position.x() && position.x() < point_x + tf_x_ + around_x) { if (pcl_vscan.at(i).y - around_y < position.y() && position.y() < pcl_vscan.at(i).y + around_y) { map_.at("vscan_points_field", *it) = 1.0; // std::exp(0.0) ; } } } } map_.setTimestamp(time.toNSec()); publish_potential_field(); }
void PotentialField::obj_callback( autoware_msgs::DetectedObjectArray::ConstPtr obj_msg) { // Create grid map. static ObstacleFieldParameter param; double ver_x_p(param.ver_x_p); double ver_y_p(param.ver_y_p); // Add data to grid map. ros::Time time = ros::Time::now(); for (GridMapIterator it(map_); !it.isPastEnd(); ++it) { Position position; map_.getPosition(*it, position); map_.at("obstacle_field", *it) = 0.0; for (int i(0); i < (int)obj_msg->objects.size(); ++i) { double pos_x = obj_msg->objects.at(i).pose.position.x + tf_x_ - map_x_offset_; double pos_y = obj_msg->objects.at(i).pose.position.y; double len_x = obj_msg->objects.at(i).dimensions.x / 2.0; double len_y = obj_msg->objects.at(i).dimensions.y / 2.0; double r, p, y; tf::Quaternion quat(obj_msg->objects.at(i).pose.orientation.x, obj_msg->objects.at(i).pose.orientation.y, obj_msg->objects.at(i).pose.orientation.z, obj_msg->objects.at(i).pose.orientation.w); tf::Matrix3x3(quat).getRPY(r, p, y); double rotated_pos_x = std::cos(-1.0 * y) * (position.x() - pos_x) - std::sin(-1.0 * y) * (position.y() - pos_y) + pos_x; double rotated_pos_y = std::sin(-1.0 * y) * (position.x() - pos_x) + std::cos(-1.0 * y) * (position.y() - pos_y) + pos_y; if (-0.5 < pos_x && pos_x < 4.0) { if (-1.0 < pos_y && pos_y < 1.0) continue; } if (pos_x - len_x < rotated_pos_x && rotated_pos_x < pos_x + len_x) { if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) { map_.at("obstacle_field", *it) = std::max(std::exp(0.0), static_cast<double>(map_.at("obstacle_field", *it))); } else if (rotated_pos_y < pos_y - len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y + len_y < rotated_pos_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } } else if (rotated_pos_x < pos_x - len_x) { if (rotated_pos_y < pos_y - len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y + len_y < rotated_pos_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_x - (pos_x - len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } } else if (pos_x + len_x < rotated_pos_x) { if (rotated_pos_y < pos_y - len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y - len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y + len_y / 2.0 < rotated_pos_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_y - (pos_y + len_y)), 2.0) / std::pow(2.0 * ver_y_p, 2.0))) + (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } else if (pos_y - len_y < rotated_pos_y && rotated_pos_y < pos_y + len_y) { map_.at("obstacle_field", *it) = std::max( std::exp( (-1.0 * (std::pow((rotated_pos_x - (pos_x + len_x)), 2.0) / std::pow(2.0 * ver_x_p, 2.0)))), static_cast<double>(map_.at("obstacle_field", *it))); } } } } // Publish grid map. map_.setTimestamp(time.toNSec()); publish_potential_field(); }