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