Skip to content

Commit

Permalink
Merge pull request #196 from gerardcanal/gcanal_dev
Browse files Browse the repository at this point in the history 
RDDL fixes and improvements
  • Loading branch information
@m312z
m312z authored Jun 20, 2019
2 parents 105cb4c + 8b1a92e commit 6280de3
Show file tree
Hide file tree
Showing 10 changed files with 177 additions and 70 deletions.
4 changes: 2 additions & 2 deletions rosplan_dependencies/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ if (LBITS EQUAL 64)
else()
SET (RDDL_CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m32")
endif(LBITS EQUAL 64)
SET (RDDL_CMAKE_CXX_FLAGS "${RDDL_CMAKE_CXX_FLAGS} -g -Wall -W -Wno-sign-compare -Wno-deprecated -ansi -pedantic -Werror -std=c++11")
SET (RDDL_CMAKE_CXX_FLAGS "${RDDL_CMAKE_CXX_FLAGS} -g -Wall -W -Wno-sign-compare -Wno-deprecated -ansi -pedantic -Werror -std=c++11 -O3 -DNDEBUG -fomit-frame-pointer")

# Lexer and parser
find_package(BISON 3.0.4 REQUIRED)
Expand Down Expand Up @@ -78,7 +78,7 @@ include_directories("rddl_parser")

add_library(rddl_parser SHARED ${BISON_RDDL_PARSER_OUTPUTS} ${FLEX_RDDL_SCANNER_OUTPUTS} ${RDDL_PARSER_SOURCES})
add_dependencies(rddl_parser ${catkin_EXPORTED_TARGETS})
SET_TARGET_PROPERTIES(rddl_parser PROPERTIES COMPILE_FLAGS "${RDDL_CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS}")
SET_TARGET_PROPERTIES(rddl_parser PROPERTIES COMPILE_FLAGS "${RDDL_CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS}" LINK_FLAGS "-g -O3")

#############
## Install ##
Expand Down
6 changes: 6 additions & 0 deletions rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,15 @@
#include "rosplan_knowledge_msgs/GetDomainOperatorService.h"
#include "rosplan_knowledge_msgs/GetDomainOperatorDetailsService.h"
#include "rosplan_knowledge_msgs/GetDomainPredicateDetailsService.h"
#include "rosplan_knowledge_msgs/GetEnumerableTypeService.h"
#include "rosplan_knowledge_msgs/DomainFormula.h"

#include "rosplan_knowledge_msgs/GetAttributeService.h"
#include "rosplan_knowledge_msgs/GetInstanceService.h"
#include "rosplan_knowledge_msgs/GetMetricService.h"
#include "rosplan_knowledge_msgs/KnowledgeItem.h"
#include "rosplan_knowledge_msgs/GetRDDLParams.h"
#include "rosplan_knowledge_msgs/GetRDDLFluentType.h"
#include "rosplan_knowledge_msgs/GetRDDLImmediateReward.h"
#include "rosplan_knowledge_msgs/SetFloat.h"
#include "rosplan_knowledge_msgs/SetInt.h"
Expand Down Expand Up @@ -78,13 +80,17 @@ namespace KCL_rosplan {
ros::ServiceServer _setRDDLMaxNonDefSrv;
ros::ServiceServer _setImmediateRewardsSrv;
ros::ServiceServer _reloadDomainStructureSrv;
ros::ServiceServer _getEnumtypesSrv;
ros::ServiceServer _getFluentTypeSrv;

bool getRDDLParams(rosplan_knowledge_msgs::GetRDDLParams::Request &req, rosplan_knowledge_msgs::GetRDDLParams::Response &res);
bool setRDDLDiscountFactor(rosplan_knowledge_msgs::SetFloat::Request &req, rosplan_knowledge_msgs::SetFloat::Response &res);
bool setRDDLHorizon(rosplan_knowledge_msgs::SetInt::Request &req, rosplan_knowledge_msgs::SetInt::Response &res);
bool setRDDLMAxNonDefActions(rosplan_knowledge_msgs::SetInt::Request &req, rosplan_knowledge_msgs::SetInt::Response &res);
bool computeImmediateReward(rosplan_knowledge_msgs::GetRDDLImmediateReward::Request &req, rosplan_knowledge_msgs::GetRDDLImmediateReward::Response &res);
bool reloadDomain(rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Request &req, rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Response &res);
bool getEnumTypes(rosplan_knowledge_msgs::GetEnumerableTypeService::Request &req, rosplan_knowledge_msgs::GetEnumerableTypeService::Response &res);
bool getFluentType(rosplan_knowledge_msgs::GetRDDLFluentType::Request &req, rosplan_knowledge_msgs::GetRDDLFluentType::Response &res);
void removeFact(const rosplan_knowledge_msgs::KnowledgeItem &msg) override;
public:
RDDLKnowledgeBase(ros::NodeHandle& n);
Expand Down
163 changes: 112 additions & 51 deletions rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp
View file

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions rosplan_knowledge_base/src/RDDLTaskParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,11 @@ namespace KCL_rosplan {
domain_name = rddlTask->domainName;
problem_name = rddlTask->name;

ROS_INFO("KCL: (%s) Pre-pocessing domain...", ros::this_node::getName().c_str());
ROS_INFO("KCL: (%s) Pre-processing domain...", ros::this_node::getName().c_str());
uninstantiated_SACs = rddlTask->SACs;
Instantiator instantiator(rddlTask);
instantiator.instantiate(false);
Preprocessor preprocessor(rddlTask, true);
Preprocessor preprocessor(rddlTask, false); // ipc2018 to false
preprocessor.preprocess(false);
ROS_INFO("KCL: (%s) RDDL domain is ready!", ros::this_node::getName().c_str());

Expand Down
2 changes: 2 additions & 0 deletions rosplan_knowledge_msgs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ add_service_files(
GetRDDLParams.srv
GetRDDLImmediateReward.srv
ReloadRDDLDomainProblem.srv
GetEnumerableTypeService.srv
GetRDDLFluentType.srv
)

## Generate added messages and services with any dependencies listed here
Expand Down
4 changes: 4 additions & 0 deletions rosplan_knowledge_msgs/srv/GetEnumerableTypeService.srv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# Get all values of the enumeration type with the name 'typeName'.
string type_name
---
string[] values
4 changes: 4 additions & 0 deletions rosplan_knowledge_msgs/srv/GetRDDLFluentType.srv
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# Get all values of the enumeration type with the name 'typeName'.
string fluent_name
---
string type
6 changes: 5 additions & 1 deletion ..._planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,9 @@
#include "rosplan_knowledge_msgs/GetDomainOperatorDetailsService.h"
#include "rosplan_knowledge_msgs/GetDomainOperatorService.h"
#include "rosplan_knowledge_msgs/GetRDDLParams.h"
#include "rosplan_knowledge_msgs/GetEnumerableTypeService.h"
#include "rosplan_knowledge_msgs/ReloadRDDLDomainProblem.h"
#include "rosplan_knowledge_msgs/GetRDDLFluentType.h"

namespace KCL_rosplan {
class RDDLProblemGenerator : public ProblemGenerator {
Expand All @@ -33,7 +35,9 @@ namespace KCL_rosplan {
std::string _domain_name;
std::string _non_fluents_name;
ros::NodeHandle _nh;
ros::ServiceClient reload_domain_;
ros::ServiceClient _reload_domain;
ros::ServiceClient _get_fluent_type;
std::map<std::string, std::vector<std::string>> _enumeration_types;
public:
RDDLProblemGenerator(const std::string& kb);
void generateProblemFile(std::string &problemPath) override;
Expand Down
3 changes: 2 additions & 1 deletion rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,8 @@ namespace KCL_rosplan {
std::istringstream p(match[2].str()); // parameters
std::string param;
while (getline(p, param, ',')) {
planner_output += " "+ param;
if (param[0] != ' ') planner_output += " ";
planner_output += param;
}
planner_output += ") [0.001]\n"; // Close parenthesis and add duration
}
Expand Down
51 changes: 38 additions & 13 deletions rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@ namespace KCL_rosplan {
}
_domain_name = nameSrv.response.domain_name;
_non_fluents_name = "nf_" + _domain_name + "__generate_instance";
reload_domain_ = _nh.serviceClient<rosplan_knowledge_msgs::ReloadRDDLDomainProblem>(kb + "/reload_rddl_domain");
_reload_domain = _nh.serviceClient<rosplan_knowledge_msgs::ReloadRDDLDomainProblem>(kb + "/reload_rddl_domain");
_get_fluent_type = _nh.serviceClient<rosplan_knowledge_msgs::GetRDDLFluentType>(kb + "/domain/fluent_type");
}


Expand All @@ -33,8 +34,8 @@ namespace KCL_rosplan {
pFile.close();
rosplan_knowledge_msgs::ReloadRDDLDomainProblem srv;
srv.request.problem_path = problemPath;
if (not reload_domain_.call(srv) or not srv.response.success) {
ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s.", reload_domain_.getService().c_str());
if (not _reload_domain.call(srv) or not srv.response.success) {
ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s.", _reload_domain.getService().c_str());
}
}

Expand All @@ -54,17 +55,30 @@ namespace KCL_rosplan {
// Print objects
pFile << "\tobjects {" << std::endl;

std::string srv_name = "/" + knowledge_base + "/domain/enumerable_type";
ros::ServiceClient getEnumerableType = _nh.serviceClient<rosplan_knowledge_msgs::GetEnumerableTypeService>(srv_name);

std::map<std::string, std::vector<std::string>> instances = getInstances();
for (auto it = instances.begin(); it != instances.end(); ++it) {
if (it->second.size() > 0) {
pFile << "\t\t" << it->first << ": " << "{";
bool comma = false;
for (auto inst_it = it->second.begin(); inst_it != it->second.end(); ++inst_it) {
if (comma) pFile << ", ";
else comma = true;
pFile << *inst_it;
if ((*it->second.begin())[0] == '@') { // If it starts with @ means it's an enumeration
rosplan_knowledge_msgs::GetEnumerableTypeService params;
params.request.type_name = it->first;
if (!getEnumerableType.call(params)) {
ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s", srv_name.c_str());
}
_enumeration_types[it->first] = params.response.values;
}
else {
pFile << "\t\t" << it->first << ": " << "{";
bool comma = false;
for (auto inst_it = it->second.begin(); inst_it != it->second.end(); ++inst_it) {
if (comma) pFile << ", ";
else comma = true;
pFile << *inst_it;
}
pFile << "};" << std::endl;
}
pFile << "};" << std::endl;
}
}
pFile << "\t};" << std::endl;
Expand Down Expand Up @@ -241,7 +255,18 @@ namespace KCL_rosplan {
pFile << ((pfit->is_negative == 0)? "true" : "false");
}
else { // FUNCTION
pFile << pfit->function_value;
rosplan_knowledge_msgs::GetRDDLFluentType gft;
gft.request.fluent_name = pfit->attribute_name;
if (!_get_fluent_type.call(gft)) {
ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s",
_get_fluent_type.getService().c_str(), gft.request.fluent_name.c_str());
return;
}
auto enum_type = _enumeration_types.find(gft.response.type);
if (enum_type != _enumeration_types.end()) {
pFile << enum_type->second[pfit->function_value];
}
else pFile << pfit->function_value;
}
pFile << ";" << std::endl;
}
Expand All @@ -254,14 +279,14 @@ namespace KCL_rosplan {
rosplan_knowledge_msgs::GetAttributeService attrSrv;
attrSrv.request.predicate_name = *it;
if (!getPropsClient.call(attrSrv)) {
ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s",
ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s",
state_proposition_service.c_str(), attrSrv.request.predicate_name.c_str());
continue;
}

// If it was not a proposition, try functions
if (attrSrv.response.attributes.size() == 0 and !getFuncsClient.call(attrSrv)) {
ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s",
ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s",
state_proposition_service.c_str(), attrSrv.request.predicate_name.c_str());
continue;
}
Expand Down

0 comments on commit 6280de3

Please sign in to comment.