3 #include <geometry_msgs/PointStamped.h> 20 virtual void updateBounds(
double origin_x,
double origin_y,
double origin_z,
double* min_x,
double* min_y,
21 double* max_x,
double* max_y)
23 boost::recursive_mutex::scoped_lock lock(
lock_);
28 for (
unsigned int i = 0; i <
people_list_.people.size(); i++)
31 people_msgs::Person tpt;
32 geometry_msgs::PointStamped pt, opt;
36 pt.point.x = person.position.x;
37 pt.point.y = person.position.y;
38 pt.point.z = person.position.z;
41 tpt.position.x = opt.point.x;
42 tpt.position.y = opt.point.y;
43 tpt.position.z = opt.point.z;
45 pt.point.x += person.velocity.x;
46 pt.point.y += person.velocity.y;
47 pt.point.z += person.velocity.z;
50 tpt.velocity.x = tpt.position.x - opt.point.x;
51 tpt.velocity.y = tpt.position.y - opt.point.y;
52 tpt.velocity.z = tpt.position.z - opt.point.z;
56 double mag =
sqrt(
pow(tpt.velocity.x, 2) +
pow(person.velocity.y, 2));
57 double factor = 1.0 + mag *
factor_;
60 *min_x = std::min(*min_x, tpt.position.x - point);
61 *min_y = std::min(*min_y, tpt.position.y - point);
62 *max_x = std::max(*max_x, tpt.position.x + point);
63 *max_y = std::max(*max_y, tpt.position.y + point);
67 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
72 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
77 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
85 boost::recursive_mutex::scoped_lock lock(
lock_);
93 std::list<people_msgs::Person>::iterator p_it;
99 people_msgs::Person person = *p_it;
100 double angle =
atan2(person.velocity.y, person.velocity.x) + 1.51;
101 double mag =
sqrt(
pow(person.velocity.x, 2) +
pow(person.velocity.y, 2));
102 double factor = 1.0 + mag *
factor_;
106 unsigned int width = std::max(1, static_cast<int>((base + point) / res)),
107 height = std::max(1, static_cast<int>((base + point) / res));
109 double cx = person.position.x, cy = person.position.y;
115 oy = cy + (point - base) *
sin(angle) - base;
120 ox = cx + (point - base) *
cos(angle) - base;
126 int start_x = 0, start_y = 0, end_x = width, end_y = height;
130 end_x = std::max(0, static_cast<int>(costmap->
getSizeInCellsX()) - dx);
132 if (static_cast<int>(start_x + dx) < min_i)
133 start_x = min_i - dx;
134 if (static_cast<int>(end_x + dx) > max_i)
140 end_y = std::max(0, static_cast<int>(costmap->
getSizeInCellsY()) - dy);
142 if (static_cast<int>(start_y + dy) < min_j)
143 start_y = min_j - dy;
144 if (static_cast<int>(end_y + dy) > max_j)
147 double bx = ox + res / 2,
149 for (
int i = start_x; i < end_x; i++)
151 for (
int j = start_y; j < end_y; j++)
153 unsigned char old_cost = costmap->
getCost(i + dx, j + dy);
157 double x = bx + i * res,
y = by + j * res;
158 double ma =
atan2(y - cy, x - cx);
161 if (fabs(diff) < M_PI / 2)
168 unsigned char cvalue = (
unsigned char) a;
169 costmap->
setCost(i + dx, j + dy, std::max(cvalue, old_cost));
double gaussian(double x, double y, double x0, double y0, double A, double varx, double vary, double skew)
void setCost(unsigned int mx, unsigned int my, unsigned char cost)
LayeredCostmap * layered_costmap_
std::list< people_msgs::Person > transformed_people_
std::string getGlobalFrameID() const
people_msgs::People people_list_
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
unsigned char getCost(unsigned int mx, unsigned int my) const
virtual void updateBounds(double origin_x, double origin_y, double origin_z, double *min_x, double *min_y, double *max_x, double *max_y)
double get_radius(double cutoff, double A, double var)
boost::recursive_mutex lock_
TFSIMD_FORCE_INLINE const tfScalar & x() const
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
unsigned int getSizeInCellsY() const
void worldToMapNoBounds(double wx, double wy, int &mx, int &my) const
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
unsigned int getSizeInCellsX() const
static const unsigned char NO_INFORMATION
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double getResolution() const
tf::TransformListener tf_
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
def shortest_angular_distance(from_angle, to_angle)