Commit 8f92598e authored by Daniel Müller's avatar Daniel Müller
Browse files

Merge branch 'dev/lower_minz_in_simulation' into 'master'

Lower min Z in simulation for downprojecting smaller obstacles on the ground

See merge request !11
parents 858ef333 f5837975
......@@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS
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;
};
......@@ -22,9 +22,11 @@
<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>
......@@ -43,6 +45,7 @@
<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.")
#---------------------------------------------------------------------------------------------------
#---------------------------------------------------------------------------------------------------
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