Commit 4cd38ce9 authored by Daniel Müller's avatar Daniel Müller
Browse files

Fixed forgotten HomerPtuLib init; Removed front slashs from frame ids used

parent 0385cd88
......@@ -5,7 +5,8 @@
HomerNavigationNode::HomerNavigationNode()
{
ros::NodeHandle nh;
homer_ptu::init(nh);
// subscribers
m_map_sub
= nh.subscribe<nav_msgs::OccupancyGrid>("/map", 3, &HomerNavigationNode::mapCallback, this);
......@@ -139,7 +140,7 @@ void HomerNavigationNode::init()
m_obstacle_position = geometry_msgs::Point();
nav_msgs::OccupancyGrid tmp_map = nav_msgs::OccupancyGrid();
tmp_map.header.frame_id = "/map";
tmp_map.header.frame_id = "map";
m_map = boost::make_shared<nav_msgs::OccupancyGrid>(tmp_map);
loadParameters();
......@@ -192,7 +193,7 @@ void HomerNavigationNode::setExplorerMap()
if (ros::Time::now() - scan.second->header.stamp < ros::Duration(1))
{
std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points(scan.second, m_transform_listener, "/map");
scan_points = map_tools::laser_msg_to_points(scan.second, m_transform_listener, "map");
for (const auto& point : scan_points)
{
Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map);
......@@ -250,7 +251,7 @@ void HomerNavigationNode::calculatePath(bool setMap)
it != waypoint_pixels.end(); ++it)
{
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id = "/map";
poseStamped.header.frame_id = "map";
poseStamped.pose.position = map_tools::fromMapCoords(*it, m_map);
poseStamped.pose.orientation.x = 0.0;
poseStamped.pose.orientation.y = 0.0;
......@@ -261,7 +262,7 @@ void HomerNavigationNode::calculatePath(bool setMap)
if (m_path_reaches_target)
{
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id = "/map";
poseStamped.header.frame_id = "map";
poseStamped.pose.position = m_target_point;
poseStamped.pose.orientation.x = 0;
poseStamped.pose.orientation.y = 0;
......@@ -297,7 +298,7 @@ void HomerNavigationNode::startNavigation()
m_waypoints.clear();
geometry_msgs::PoseStamped poseStamped = geometry_msgs::PoseStamped();
poseStamped.header.frame_id = "/map";
poseStamped.header.frame_id = "map";
poseStamped.pose.position = m_target_point;
poseStamped.pose.orientation.w = 1;
m_waypoints.push_back(poseStamped);
......@@ -373,14 +374,14 @@ void HomerNavigationNode::startNavigation()
void HomerNavigationNode::sendPathData()
{
nav_msgs::Path msg;
msg.header.frame_id = "/map";
msg.header.frame_id = "map";
msg.header.stamp = ros::Time::now();
if (m_waypoints.size() > 0)
{
msg.poses = m_waypoints;
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.pose = m_robot_pose;
pose_stamped.header.frame_id = "/map";
pose_stamped.header.frame_id = "map";
pose_stamped.header.stamp = ros::Time::now();
msg.poses.insert(msg.poses.begin(), pose_stamped);
}
......@@ -422,7 +423,7 @@ void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
homer_mapnav_msgs::TargetUnreachable unreachable_msg;
unreachable_msg.reason = reason;
unreachable_msg.point = geometry_msgs::PointStamped();
unreachable_msg.point.header.frame_id = "/map";
unreachable_msg.point.header.frame_id = "map";
unreachable_msg.point.point = m_obstacle_position;
m_target_unreachable_pub.publish(unreachable_msg);
m_waypoints.clear();
......@@ -507,7 +508,7 @@ bool HomerNavigationNode::obstacleOnPath()
if (ros::Time::now() - scan.second->header.stamp < ros::Duration(1))
{
std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points(scan.second, m_transform_listener, "/map");
scan_points = map_tools::laser_msg_to_points(scan.second, m_transform_listener, "map");
filterScanPoints(scan_points);
geometry_msgs::Point lp;
......@@ -1212,7 +1213,7 @@ bool HomerNavigationNode::backwardObstacle()
base_link_point.x = x[i];
base_link_point.y = y[i];
map_coord = map_tools::toMapCoords(
map_tools::transformPoint(base_link_point, m_transform_listener, "/base_link", "/map"),
map_tools::transformPoint(base_link_point, m_transform_listener, "base_link", "map"),
m_map);
map_point.x = map_coord.x();
map_point.y = map_coord.y();
......@@ -1357,7 +1358,7 @@ void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstP
m_scan_map[msg->header.frame_id] = msg;
if (m_state != IDLE)
{
std::vector<geometry_msgs::Point> points = map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link");
std::vector<geometry_msgs::Point> points = map_tools::laser_msg_to_points(msg, m_transform_listener, "base_link");
if(points.size() > 0)
{
m_last_laser_time = ros::Time::now();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment