diff --git a/doc/stl/Pepper_internal_USB_connector.jpg b/doc/stl/Pepper_internal_USB_connector.jpg new file mode 100644 index 00000000..481de755 Binary files /dev/null and b/doc/stl/Pepper_internal_USB_connector.jpg differ diff --git a/doc/stl/URG_Stay.png b/doc/stl/URG_Stay.png new file mode 100755 index 00000000..c7af636b Binary files /dev/null and b/doc/stl/URG_Stay.png differ diff --git a/doc/stl/URG_Stay_Base.stl b/doc/stl/URG_Stay_Base.stl new file mode 100755 index 00000000..8a39a66e Binary files /dev/null and b/doc/stl/URG_Stay_Base.stl differ diff --git a/doc/stl/URG_Stay_Tower.stl b/doc/stl/URG_Stay_Tower.stl new file mode 100755 index 00000000..6a8788f2 Binary files /dev/null and b/doc/stl/URG_Stay_Tower.stl differ diff --git a/share/urdf/pepper.urdf b/share/urdf/pepper.urdf index a97cf156..f2196d90 100644 --- a/share/urdf/pepper.urdf +++ b/share/urdf/pepper.urdf @@ -1461,4 +1461,11 @@ + + + + + + + diff --git a/src/converters/laser.cpp b/src/converters/laser.cpp old mode 100644 new mode 100755 index dc29a9ba..e550dab6 --- a/src/converters/laser.cpp +++ b/src/converters/laser.cpp @@ -130,7 +130,8 @@ static const char* laserMemoryKeys[] = { LaserConverter::LaserConverter( const std::string& name, const float& frequency, const qi::SessionPtr& session ): BaseConverter( name, frequency, session ), - p_memory_(session->service("ALMemory")) + p_memory_(session->service("ALMemory")), + has_ext_laser(false) { } @@ -141,6 +142,35 @@ void LaserConverter::registerCallback( message_actions::MessageAction action, Ca void LaserConverter::callAll( const std::vector& actions ) { + // If Pepper has an external URG + if(has_ext_laser){ + qi::AnyValue anyvalues; + try{ + anyvalues = p_memory_.call("getData", "Device/Laser/Value"); + } catch (const std::exception& e) { + std::cerr << "Could not get data from external LASER sensor. " << std::endl; + } + + size_t len = anyvalues.size(); + + msg_.header.stamp = ros::Time::now(); + + for( size_t i=0; i laser_keys_value(laserMemoryKeys, laserMemoryKeys+90); std::vector result_value; @@ -212,13 +242,44 @@ void LaserConverter::callAll( const std::vector& void LaserConverter::reset( ) { - msg_.header.frame_id = "base_footprint"; - msg_.angle_min = -2.0944; // -120 - msg_.angle_max = 2.0944; // +120 - msg_.angle_increment = (2*2.0944) / (15+15+15+8+8); // 240 deg FoV / 61 points (blind zones inc) - msg_.range_min = 0.1; // in m - msg_.range_max = 1.5; // in m - msg_.ranges = std::vector(61, -1.0f); + try{ + qi::AnyValue anyvalues = p_memory_.call("getData", "Device/Laser/Value"); + has_ext_laser = true; + } catch (const std::exception& e) { + std::cerr << "Could not get data from external LASER sensor. " << std::endl; + } + + if(has_ext_laser){ + try{ + p_laser_ = session_->service("ALLaser"); + } catch (const std::exception& e) { + std::cerr << "Could not get ALLaser. " << std::endl; + } + + try{ + p_laser_.call("setOpeningAngle", -2.0923, 2.0923); + p_laser_.call("setDetectingLength", 20, 5600); + p_laser_.call("laserON"); + } catch (const std::exception& e) { + std::cerr << "Could not set laser angle. " << std::endl; + } + + msg_.header.frame_id = "ExternalLaser_frame"; + msg_.angle_min = -2.0923; // -120 + msg_.angle_max = 2.0923; // +120 + msg_.angle_increment = (2*2.0923) / 683; // 240 deg FoV / 683 points + msg_.range_min = 0.02; // in m + msg_.range_max = 5.6; // in m + msg_.ranges = std::vector(1024, -1.0f); + }else{ + msg_.header.frame_id = "base_footprint"; + msg_.angle_min = -2.0944; // -120 + msg_.angle_max = 2.0944; // +120 + msg_.angle_increment = (2*2.0944) / (15+15+15+8+8); // 240 deg FoV / 61 points (blind zones inc) + msg_.range_min = 0.1; // in m + msg_.range_max = 1.5; // in m + msg_.ranges = std::vector(61, -1.0f); + } } } //converter diff --git a/src/converters/laser.hpp b/src/converters/laser.hpp old mode 100644 new mode 100755 index 3c77cf9e..2f3d162e --- a/src/converters/laser.hpp +++ b/src/converters/laser.hpp @@ -51,9 +51,12 @@ class LaserConverter : public BaseConverter private: qi::AnyObject p_memory_; + qi::AnyObject p_laser_; std::map callbacks_; sensor_msgs::LaserScan msg_; + + bool has_ext_laser; }; // class } //publisher