Implement setting of light attenuation via ROS Param Server

Set as rosparam set <lightname>/attenuation/<type> Where <lightname> can be for example /ambf/env/lights/light1 and type is constant/linear/quadratic Can also query the light params using rosparam get
WPI-AIM · Mar 15, 2024 · 054534d · 054534d
1 parent 895dfe0
commit 054534d
Show file tree

Hide file tree

Showing 5 changed files with 56 additions and 4 deletions.
diff --git a/ambf_framework/afFramework.cpp b/ambf_framework/afFramework.cpp
@@ -7217,9 +7217,9 @@ bool afLight::createFromAttribs(afLightAttributes *a_attribs)
     m_spotLight->setShadowMapEnabled(true);
 
     if (a_attribs->m_attenuationDefined){
-        m_spotLight->setAttConstant(a_attribs->m_constantAttenuation);
-        m_spotLight->setAttLinear(a_attribs->m_linearAttenuation);
-        m_spotLight->setAttQuadratic(a_attribs->m_quadraticAttenuation);
+        setConstantAttenuation(a_attribs->m_constantAttenuation);
+        setLinearAttenuation(a_attribs->m_linearAttenuation);
+        setQuadraticAttenuation(a_attribs->m_quadraticAttenuation);
     }
 
     switch (a_attribs->m_shadowQuality) {

diff --git a/ambf_framework/afFramework.h b/ambf_framework/afFramework.h
@@ -2083,6 +2083,18 @@ class afLight: public afBaseObject{
 
     cGenericLight* getInternalLight();
 
+    double getConstantAttenuation(){return m_spotLight->getAttConstant();}
+
+    double getLinearAttenuation(){return m_spotLight->getAttLinear();}
+
+    double getQuadraticAttenuation(){return m_spotLight->getAttQuadratic();}
+
+    void setConstantAttenuation(double att){m_spotLight->setAttConstant(att);}
+
+    void setLinearAttenuation(double att){m_spotLight->setAttLinear(att);}
+
+    void setQuadraticAttenuation(double att){m_spotLight->setAttQuadratic(att);}
+
 protected:
     cSpotLight* m_spotLight;
 

diff --git a/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp b/ambf_plugins/core/ros_comm_plugin/ObjectCommPlugin.cpp
@@ -532,6 +532,10 @@ void afObjectCommunicationPlugin::lightFetchCommand(afLightPtr lightPtr, double
             lightPtr->setCutOffAngle(cutoff_angle);
 
             lightPtr->resolveParent(parent_name);
+
+            lightPtr->setConstantAttenuation(m_lightCommPtr->get_constant_attenuation());
+            lightPtr->setLinearAttenuation(m_lightCommPtr->get_linear_attenuation());
+            lightPtr->setQuadraticAttenuation(m_lightCommPtr->get_quadratic_attenuation());
         }
 
         m_read_count = 0;
@@ -544,6 +548,9 @@ void afObjectCommunicationPlugin::lightUpdateState(afLightPtr lightPtr, double d
         m_lightCommPtr->set_cuttoff_angle(lightPtr->getCutOffAngle());
         m_lightCommPtr->set_type(ambf_comm::LightType::SPOT);
         m_lightCommPtr->set_parent_name(lightPtr->m_parentName);
+        m_lightCommPtr->set_attenuation(lightPtr->getConstantAttenuation(),
+                                        lightPtr->getLinearAttenuation(),
+                                        lightPtr->getQuadraticAttenuation());
 
         m_lightCommPtr->set_params_on_server();
         m_paramsSet = true;

diff --git a/ambf_ros_modules/ambf_server/include/ambf_server/Light.h b/ambf_ros_modules/ambf_server/include/ambf_server/Light.h
@@ -59,7 +59,8 @@ enum class LightType{
 enum class LightParamsEnum{
     cuttoff_angle,
     parent_name,
-    type
+    type,
+    attenuation
 };
 
 
@@ -76,10 +77,14 @@ class LightParams{
     // Setters
     void set_type(LightType val){m_light_type = val;}
     void set_cuttoff_angle(double val){m_cuttoff_angle = val;}
+    void set_attenuation(double cons, double lin, double quad);
 
     // Getters
     LightType get_type(){return m_light_type;}
     double get_cuttoff_angle(){return m_cuttoff_angle;}
+    double get_constant_attenuation(){return m_attenuation["constant"];}
+    double get_linear_attenuation(){return m_attenuation["linear"];}
+    double get_quadratic_attenuation(){return m_attenuation["quadratic"];}
 
     // This a flag to check if any param has been updated
     bool m_paramsChanged;
@@ -92,6 +97,7 @@ class LightParams{
     // Datatyped Variables for params defined on the server
     double m_type;
     double m_cuttoff_angle;
+    std::map<std::string, double> m_attenuation; // Constant, Linear and Quadratic Attenuation
     LightType m_light_type;
 };
 

diff --git a/ambf_ros_modules/ambf_server/src/Light.cpp b/ambf_ros_modules/ambf_server/src/Light.cpp
@@ -56,28 +56,42 @@ const std::string light_param_enum_to_str(LightParamsEnum enumVal)
     if (enumVal == LightParamsEnum::cuttoff_angle) return "cutoff_angle";
     else if (enumVal == LightParamsEnum::parent_name) return "parent_name";
     else if (enumVal == LightParamsEnum::type) return "type";
+    else if (enumVal == LightParamsEnum::attenuation) return "attenuation";
     return "";
 }
 
 LightParams::LightParams(){
     m_paramsChanged = false;
+    m_attenuation["constant"] = 1.0;
+    m_attenuation["linear"] = 0.0;
+    m_attenuation["quadratic"] = 0.0;
+}
+
+void LightParams::set_attenuation(double cons, double lin, double quad)
+{
+    m_attenuation["constant"] = cons;
+    m_attenuation["linear"] = lin;
+    m_attenuation["quadratic"] = quad;
 }
 
 void Light::set_params_on_server(){
     nodePtr->setParam(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::cuttoff_angle), m_cuttoff_angle);
     nodePtr->setParam(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::parent_name), m_State.parent_name.data);
     nodePtr->setParam(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::type), light_type_enum_to_str(m_light_type));
+    nodePtr->setParam(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::attenuation), m_attenuation);
 }
 
 void Light::update_params_from_server(){
     double ca;
     std::string pn;
     std::string lt;
+    std::map<std::string, double> att;
     LightType lt_enum;
 
     nodePtr->getParamCached(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::cuttoff_angle), ca);
     nodePtr->getParamCached(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::parent_name), pn);
     nodePtr->getParamCached(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::type), lt);
+    nodePtr->getParamCached(m_base_prefix + "/" + light_param_enum_to_str(LightParamsEnum::attenuation), att);
 
     if (lt.compare(light_type_enum_to_str(LightType::SPOT)) == 0){
         lt_enum = LightType::SPOT;
@@ -106,6 +120,19 @@ void Light::update_params_from_server(){
         std::cerr << "INFO! PARAMS CHANGED FOR \"" << m_name << "\"\n";
     }
 
+    std::map<std::string, double>::iterator it;
+    for (it = m_attenuation.begin() ; it != m_attenuation.end() ; ++it){
+        try{
+            if (att[it->first] != it->second){
+                m_paramsChanged = true;
+                it->second = att[it->first];
+            }
+        }
+        catch (...){
+            // Do nothing
+        }
+    }
+
     // Finally update the local copies of the params
     m_cuttoff_angle = ca;
     m_State.parent_name.data = pn;