Commit 613942f8 authored by Daniel Müller's avatar Daniel Müller
Browse files

Added config param to disable path simplification (post-processing), new...

Added config param to disable path simplification (post-processing), new default is disabled simplification for navigating in complex surroundings
parent 7e65d3cc
......@@ -41,3 +41,4 @@
/homer_navigation/use_ptu: false # bool toggles if the ptu is being used to look at the next Waypoint during navigation
/homer_navigation/unknown_threshold: 70 # obstacle strenght under which the obstacle is ignored by navigation
/homer_navigation/waypoint_radius_factor: 0.5 # factor for includance of obstacle map
/homer_navigation/sample_path: false # Downsample the amount of waypoints
......@@ -315,6 +315,7 @@ private:
int m_unknown_threshold;
bool m_use_ptu;
bool m_sample_path = false;
geometry_msgs::Twist m_cmd_vel;
......
......@@ -247,33 +247,27 @@ void HomerNavigationNode::calculatePath(bool setMap)
{
ROS_DEBUG_STREAM("Found tmp path with " << tmp_pixel_path.size() << " points.");
m_obstacle_on_path = false;
std::vector<Eigen::Vector2i> waypoint_pixels
= m_explorer->sampleWaypointsFromPath(tmp_pixel_path, m_waypoint_sampling_threshold);
m_sample_path = false;
ros::param::getCached("/homer_navigation/sample_path", m_sample_path);
const std::vector<Eigen::Vector2i>& waypoint_pixels =
(m_sample_path) ? m_explorer->sampleWaypointsFromPath(tmp_pixel_path, m_waypoint_sampling_threshold) : tmp_pixel_path;
m_waypoints.clear();
ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoint_pixels.size());
if (waypoint_pixels.size() > 0)
{
for (std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin();
it != waypoint_pixels.end(); ++it)
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id = "map";
poseStamped.pose.orientation.w = 1.0;
for(const auto& wp : waypoint_pixels)
{
geometry_msgs::PoseStamped poseStamped;
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;
poseStamped.pose.orientation.z = 0.0;
poseStamped.pose.orientation.w = 1.0;
poseStamped.pose.position = map_tools::fromMapCoords(wp, m_map);
m_waypoints.push_back(poseStamped);
}
if (m_path_reaches_target)
{
geometry_msgs::PoseStamped poseStamped;
poseStamped.header.frame_id = "map";
poseStamped.pose.position = m_target_point;
poseStamped.pose.orientation.x = 0;
poseStamped.pose.orientation.y = 0;
poseStamped.pose.orientation.z = 0;
poseStamped.pose.orientation.w = 1;
m_waypoints.push_back(poseStamped);
}
sendPathData();
......@@ -416,7 +410,7 @@ void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
}
else if (ros::Time::now() - m_unreachable_delay > ros::Duration(unreachable_duration))
{
bool m_use_ptu = false;
m_use_ptu = false;
ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
if (m_use_ptu)
{
......@@ -854,7 +848,7 @@ bool HomerNavigationNode::checkWaypoints()
sendPathData();
bool m_use_ptu = false;
m_use_ptu = false;
ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
if (m_use_ptu)
{
......@@ -1121,7 +1115,7 @@ void HomerNavigationNode::avoidingCollision()
void HomerNavigationNode::finalTurn()
{
bool m_use_ptu = false;
m_use_ptu = false;
ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
if (m_use_ptu)
{
......
Supports Markdown
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