5 #include <geometry_msgs/PointStamped.h> 26 boost::recursive_mutex::scoped_lock lock(
lock_);
32 double* max_x,
double* max_y)
34 boost::recursive_mutex::scoped_lock lock(
lock_);
39 for (
unsigned int i = 0; i <
people_list_.people.size(); i++)
42 people_msgs::Person tpt;
43 geometry_msgs::PointStamped pt, opt;
47 pt.point.x = person.position.x;
48 pt.point.y = person.position.y;
49 pt.point.z = person.position.z;
52 tpt.position.x = opt.point.x;
53 tpt.position.y = opt.point.y;
54 tpt.position.z = opt.point.z;
56 pt.point.x += person.velocity.x;
57 pt.point.y += person.velocity.y;
58 pt.point.z += person.velocity.z;
61 tpt.velocity.x = opt.point.x - tpt.position.x;
62 tpt.velocity.y = opt.point.y - tpt.position.y;
63 tpt.velocity.z = opt.point.z - tpt.position.z;
69 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
74 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
79 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
94 double a = *min_x, b = *min_y, c = *max_x,
d = *max_y;
LayeredCostmap * layered_costmap_
std::list< people_msgs::Person > transformed_people_
std::string getGlobalFrameID() const
people_msgs::People people_list_
virtual void updateBoundsFromPeople(double *min_x, double *min_y, double *max_x, double *max_y)=0
ros::Subscriber people_sub_
static const unsigned char FREE_SPACE
virtual void updateBounds(double origin_x, double origin_y, double origin_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
boost::recursive_mutex lock_
void peopleCallback(const people_msgs::People &people)
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
tf::TransformListener tf_
virtual void onInitialize()