Commit 7946ebca authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

Merge branch 'master' of gitlab.uni-koblenz.de:robbie/homer_navigation

parents 3c8b4178 7519d856
......@@ -2,6 +2,24 @@
Changelog for package homer_navigation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.107 (2021-06-03)
--------------------
* Fixed forgotten HomerPtuLib init; Removed front slashs from frame ids used
* Contributors: Daniel Müller
0.1.106 (2021-06-03)
--------------------
* Adjusted to homer_ptu_msgs update and integrated HomerPtuLib for future updates
* Contributors: Daniel Müller
0.1.105 (2021-05-14)
--------------------
* Merge branch 'dev/lower_minz_in_simulation' into 'master'
Lower min Z in simulation for downprojecting smaller obstacles on the ground
See merge request robbie/homer_navigation!11
* Lower min Z in simulation in depth_occupancy_map for downprojecting smaller obstacles on the ground; Added Python interface function for toggling feature on/off; Minor clean up
* Contributors: Daniel Müller
0.1.101 (2020-11-12)
--------------------
* Merge branch 'cpp_lib_refactor' into 'master'
......
......@@ -15,12 +15,14 @@ find_package(catkin REQUIRED COMPONENTS
homer_msgs
tf
cmake_modules
homer_ptu_msgs
homer_ptu
actionlib
homer_robbie_architecture
trajectory_msgs
homer_tools
homer_tts
homer_rgbd_node
pcl_ros
)
find_package(OpenCV REQUIRED)
......
......@@ -31,7 +31,6 @@
#include <tf/transform_listener.h>
#include <homer_nav_libs/tools.h>
#include <homer_tools/coordinate_converstion.h>
class depth_occupancy_map
{
......@@ -85,4 +84,6 @@ private:
bool m_got_map_size;
const double MAX_DISTANCE;
std::string m_laser_frame;
bool m_is_simulation;
};
......@@ -18,8 +18,6 @@
#include <homer_mapnav_msgs/NavigateToPOI.h>
#include <homer_mapnav_msgs/StartNavigation.h>
#include <homer_mapnav_msgs/TargetUnreachable.h>
#include <homer_ptu_msgs/CenterWorldPoint.h>
#include <homer_ptu_msgs/SetPanTilt.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/LaserScan.h>
......
<?xml version="1.0"?>
<package>
<name>homer_navigation</name>
<version>0.1.104</version>
<version>0.1.107</version>
<description>The homer_navigation package</description>
<maintainer email="fpolster@uni-koblenz.de">Florian Polster</maintainer>
<maintainer email="raphael@uni-koblenz.de">Raphael Memmesheimer</maintainer>
<maintainer email="muellerd@uni-koblenz.de">Daniel Müller</maintainer>
<author email="mknauf@uni-koblenz.de">Malte Knauf</author>
<license>GPLv3</license>
......@@ -17,14 +16,16 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>homer_mapnav_msgs</build_depend>
<build_depend>homer_nav_libs</build_depend>
<build_depend>homer_ptu_msgs</build_depend>
<build_depend>homer_ptu</build_depend>
<build_depend>homer_msgs</build_depend>
<build_depend>homer_robbie_architecture</build_depend>
<build_depend>homer_tools</build_depend>
<build_depend>homer_tts</build_depend>
<build_depend>homer_rgbd_node</build_depend>
<build_depend>move_base_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>sensor_msgs</build_depend>
......@@ -39,10 +40,11 @@
<run_depend>eigen</run_depend>
<run_depend>homer_mapnav_msgs</run_depend>
<run_depend>homer_nav_libs</run_depend>
<run_depend>homer_ptu_msgs</run_depend>
<run_depend>homer_ptu</run_depend>
<run_depend>homer_msgs</run_depend>
<run_depend>homer_robbie_architecture</run_depend>
<run_depend>homer_tts</run_depend>
<run_depend>homer_rgbd_node</run_depend>
<run_depend>move_base_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>roscpp</run_depend>
......
#include <homer_navigation/depth_occupancy_map.h>
#include <homer_rgbd_node/GetRGBDData.h>
#include <map>
#include <math.h>
#include <numeric>
#include <pcl_ros/transforms.h>
depth_occupancy_map::depth_occupancy_map(ros::NodeHandle* nh)
: MAX_DISTANCE(4.0)
{
std::string env_sim = std::getenv("SIMULATION");
ROS_INFO_STREAM("env_sim: " << env_sim);
m_is_simulation = env_sim.compare("True") == 0 || env_sim.compare("1") == 0;
ROS_INFO_STREAM("is simulation: " << m_is_simulation);
m_nh = nh;
m_MapSubscriber = nh->subscribe("/map", 1, &depth_occupancy_map::map_callback, this);
std::string scan_topic;
......@@ -45,7 +52,7 @@ depth_occupancy_map::depth_occupancy_map(ros::NodeHandle* nh)
ros::param::getCached("/depth_mapping/do_mapping", m_do_mapping);
m_navigating = false;
m_depthSubscribed = false;
ros::param::get("/rgbd_node/points_topic", m_depth_topic);
m_depth_topic = homer_rgbd_node::get_point_cloud_topic("front", "low");
ROS_INFO_STREAM("[depth_occupancy_map] points topic: " << m_depth_topic);
m_timer.start();
......@@ -152,7 +159,7 @@ void depth_occupancy_map::updateDepthData(const sensor_msgs::PointCloud2::ConstP
float min_distance = 0.45;
ros::param::getCached("/depth_mapping/min_distance", min_distance);
const float max_range_z = 1.6;
const float min_range_z = 0.2;
const float min_range_z = (m_is_simulation) ? 0.01 : 0.2;
const float max_distance = MAX_DISTANCE * MAX_DISTANCE;
const float min_distance_squared = min_distance * min_distance;
geometry_msgs::Point point;
......
......@@ -6,7 +6,8 @@ from homer_mapnav_msgs.msg import DriveToAction, DriveToGoal
#---------------------------------------------------------------------------------------------------
class NavigationInterface:
#---------------------------------------------------------------------------------------------------
def __drive_to_POI(self, poi_name, distance_to_target, suppress_speaking, skip_final_turn, \
@staticmethod
def __drive_to_POI(poi_name, distance_to_target, suppress_speaking, skip_final_turn, \
stop_before_obstacle, check_obstacle_type, keep_torso_position):
client = actionlib.SimpleActionClient('drive_to', DriveToAction)
......@@ -26,7 +27,8 @@ class NavigationInterface:
#---------------------------------------------------------------------------------------------------
def drive_to_POI(self, poi_name, distance_to_target=0.03, suppress_speaking=False, \
@staticmethod
def drive_to_POI(poi_name, distance_to_target=0.03, suppress_speaking=False, \
skip_final_turn=False, stop_before_obstacle=False, check_obstacle_type=False, \
callback=None, keep_torso_position=False):
"""
......@@ -54,14 +56,15 @@ class NavigationInterface:
:keep_torso_position: If `True` tiagos torso position will stay unchanged
"""
rospy.loginfo("[Driving] to " + poi_name)
client, goal = self.__drive_to_POI(poi_name, distance_to_target, suppress_speaking, \
client, goal = NavigationInterface.__drive_to_POI(poi_name, distance_to_target, suppress_speaking, \
skip_final_turn, stop_before_obstacle, check_obstacle_type, \
keep_torso_position=keep_torso_position)
client.send_goal(goal, callback)
#---------------------------------------------------------------------------------------------------
def drive_to_POI_blocked(self, poi_name, distance_to_target=0.03, suppress_speaking=False, \
@staticmethod
def drive_to_POI_blocked(poi_name, distance_to_target=0.03, suppress_speaking=False, \
skip_final_turn=False, stop_before_obstacle=False, check_obstacle_type=False, \
timeout=rospy.Duration(0), keep_torso_position=False):
"""
......@@ -93,7 +96,7 @@ class NavigationInterface:
"""
rospy.loginfo("[Robot][DriveToBlocked] to " + poi_name)
client, goal = self.__drive_to_POI(poi_name, distance_to_target, suppress_speaking, \
client, goal = NavigationInterface.__drive_to_POI(poi_name, distance_to_target, suppress_speaking, \
skip_final_turn, stop_before_obstacle, check_obstacle_type, \
keep_torso_position=keep_torso_position)
client.send_goal(goal)
......@@ -102,5 +105,16 @@ class NavigationInterface:
return None
return client.get_result()
#---------------------------------------------------------------------------------------------------
@staticmethod
def enable_depth_occupancy_mapping():
rospy.set_param("/depth_mapping/do_mapping", True)
rospy.loginfo("Enabled depth occupancy mapping.")
@staticmethod
def disable_depth_occupancy_mapping():
rospy.set_param("/depth_mapping/do_mapping", False)
rospy.loginfo("Enabled depth occupancy mapping.")
#---------------------------------------------------------------------------------------------------
#---------------------------------------------------------------------------------------------------
#include "homer_navigation/homer_navigation_node.h"
#include <homer_ptu/HomerPtuLib.h>
HomerNavigationNode::HomerNavigationNode()
{
ros::NodeHandle nh;
homer_ptu::init(nh);
// subscribers
m_map_sub
= nh.subscribe<nav_msgs::OccupancyGrid>("/map", 3, &HomerNavigationNode::mapCallback, this);
......@@ -45,9 +48,6 @@ HomerNavigationNode::HomerNavigationNode()
m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>(
"/homer_navigation/target_unreachable", 3);
m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 3);
m_ptu_center_world_point_pub
= nh.advertise<homer_ptu_msgs::CenterWorldPoint>("/ptu/center_world_point", 3);
m_set_pan_tilt_pub = nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 3);
m_get_POIs_client
= nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>("/map_manager/get_pois");
......@@ -140,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();
......@@ -193,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);
......@@ -251,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;
......@@ -262,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;
......@@ -298,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);
......@@ -374,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);
}
......@@ -415,18 +415,15 @@ void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
if (m_use_ptu)
{
// reset PTU
homer_ptu_msgs::SetPanTilt msg;
msg.absolute = true;
msg.panAngle = 0;
msg.tiltAngle = 0;
m_set_pan_tilt_pub.publish(msg);
homer_ptu::stop_focussing_view();
homer_ptu::set_ptu(0, 0);
}
m_state = IDLE;
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();
......@@ -511,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;
......@@ -684,13 +681,11 @@ bool HomerNavigationNode::checkWaypoints()
ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
if (m_use_ptu)
{
ROS_DEBUG_STREAM("Moving PTU to center next Waypoint");
homer_ptu_msgs::CenterWorldPoint ptu_msg;
ptu_msg.point.x = m_waypoints[0].pose.position.x;
ptu_msg.point.y = m_waypoints[0].pose.position.y;
ptu_msg.point.z = 0;
ptu_msg.permanent = true;
m_ptu_center_world_point_pub.publish(ptu_msg);
ROS_DEBUG_STREAM("Permanently focussing PTU on center next navigation waypoint.");
geometry_msgs::PointStamped ps;
ps.point = m_waypoints[0].pose.position;
ps.header = m_waypoints[0].header;
homer_ptu::focus_view(ps, true);
}
return true;
......@@ -953,11 +948,8 @@ void HomerNavigationNode::finalTurn()
if (m_use_ptu)
{
// reset PTU
homer_ptu_msgs::SetPanTilt msg;
msg.absolute = true;
msg.panAngle = 0;
msg.tiltAngle = 0;
m_set_pan_tilt_pub.publish(msg);
homer_ptu::stop_focussing_view();
homer_ptu::set_ptu(0, 0);
}
if (m_skip_final_turn)
......@@ -1221,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();
......@@ -1375,7 +1367,7 @@ void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstP
m_scan_map[msg->header.frame_id] = tmp_scan;
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