Skip to content

Commit

Permalink
got rid of boolean flags bouncing between the different threads and s…
Browse files Browse the repository at this point in the history 
…witched to a signals/slots implementation.
  • Loading branch information
@jvarley
jvarley committed Jul 14, 2016
1 parent 1c86925 commit 926e4eb
Show file tree
Hide file tree
Showing 3 changed files with 71 additions and 96 deletions.
16 changes: 14 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,12 +151,22 @@ catkin_package(
## Build ##
###########

include(${QT_USE_FILE})
set(CMAKE_INCLUDE_CURRENT_DIR ON)

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include
${catkin_INCLUDE_DIRS}
${QT_INCLUDES})

ADD_DEFINITIONS(${QT_DEFINITIONS})

set(MOCS
include/graspit_interface.h)

qt4_wrap_cpp(GENERATED_SOURCES ${MOCS})

## Declare a C++ library
# add_library(graspit_interface
# src/${PROJECT_NAME}/graspit_interface.cpp
Expand All @@ -170,11 +180,13 @@ include_directories(include
## Declare a C++ executable
add_library(graspit_interface
src/graspit_interface.cpp
src/main.cpp)
src/main.cpp
${GENERATED_SOURCES}
${MOCS})

## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(graspit_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(graspit_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${QT_DEFINITIONS})

## Specify libraries to link a library or executable target against
target_link_libraries(graspit_interface
Expand Down
27 changes: 15 additions & 12 deletions include/graspit_interface.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,11 @@
namespace GraspitInterface
{

class GraspitInterface : public Plugin
class GraspitInterface : public QObject, public Plugin
{

Q_OBJECT

private:
ros::NodeHandle *nh;

Expand Down Expand Up @@ -97,13 +99,8 @@ class GraspitInterface : public Plugin

GraspPlanningState *mHandObjectState;
SimAnnPlanner *mPlanner;
bool startPlanner;
bool plannerStarted;
bool buildPlannerResponse;
bool finishedBuildingResponse;

bool requestRender;

bool firstTimeInMainLoop;

// Service callbacks
bool getRobotCB(graspit_interface::GetRobot::Request &request,
Expand Down Expand Up @@ -185,11 +182,6 @@ class GraspitInterface : public Plugin
//ActionServer callbacks
void PlanGraspsCB(const graspit_interface::PlanGraspsGoalConstPtr &goal);

//the planner must be started in the mainloop, not the action server callback
void startPlannerInMainLoop();
void renderInMainLoop();
void buildPlannerResponseInMainLoop();

public:
GraspitInterface(){}
~GraspitInterface(){}
Expand All @@ -198,6 +190,17 @@ class GraspitInterface : public Plugin

virtual int mainLoop();

public Q_SLOTS:

void runPlannerInMainThread();
void processPlannerResultsInMainThread();

Q_SIGNALS:

void emitRunPlannerInMainThread();
void emitProcessPlannerResultsInMainThread();


};

}
Expand Down
124 changes: 42 additions & 82 deletions src/graspit_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,11 +57,8 @@ int GraspitInterface::init(int argc, char** argv)
boost::bind(&GraspitInterface::PlanGraspsCB, this, _1), false);
plan_grasps_as->start();

startPlanner = false;
plannerStarted = false;
requestRender = false;
buildPlannerResponse = false;
finishedBuildingResponse = true;
firstTimeInMainLoop = true;

mPlanner = NULL;
mHandObjectState = NULL;

Expand All @@ -72,24 +69,15 @@ int GraspitInterface::init(int argc, char** argv)

int GraspitInterface::mainLoop()
{
//Planner Must be started by mainthread, so it cannot be
//Started inside the callback for the action server.
if(startPlanner)
{
startPlannerInMainLoop();
}

if(buildPlannerResponse)
{
buildPlannerResponseInMainLoop();
}

//Rendering must be handled my the main thread
//So if one of the action servers wants a render
//They set requestRender to true, and it will be rendered here.
if(requestRender)
if(firstTimeInMainLoop)
{
renderInMainLoop();
//Planner Must be started by mainthread, so it cannot be
//Started inside the callback for the action server. I need to connect these here
//So that when the signal is emitted, the slot function is executed by the correct thread.
QObject::connect(this, SIGNAL(emitRunPlannerInMainThread()), this, SLOT(runPlannerInMainThread()), Qt::BlockingQueuedConnection);
QObject::connect(this, SIGNAL(emitProcessPlannerResultsInMainThread()), this, SLOT(processPlannerResultsInMainThread()), Qt::BlockingQueuedConnection);
firstTimeInMainLoop = false;
ROS_INFO("Planner Signal/Slots connected");
}

ros::spinOnce();
Expand Down Expand Up @@ -544,25 +532,8 @@ bool GraspitInterface::findInitialContactCB(graspit_interface::FindInitialContac
void GraspitInterface::PlanGraspsCB(const graspit_interface::PlanGraspsGoalConstPtr &_goal)
{
goal = *_goal;

Hand *mHand = graspitCore->getWorld()->getCurrentHand();
if(mHand == NULL)
{
ROS_INFO("Planning Hand is NULL");
}
GraspableBody *mObject = graspitCore->getWorld()->getGB(0);
if(mObject == NULL)
{
ROS_INFO("Planning Object is NULL");
}

startPlanner = true;

ROS_INFO("Waiting For Planner to Start");
while(!plannerStarted)
{
sleep(1.0);
}
ROS_INFO("About to Call emit runPlannerInMainLoop();");
emit emitRunPlannerInMainThread();

ROS_INFO("Waiting For Planner to Finish");
while(mPlanner->isActive())
Expand All @@ -576,29 +547,16 @@ void GraspitInterface::PlanGraspsCB(const graspit_interface::PlanGraspsGoalConst
plan_grasps_as->publishFeedback(feedback_);
}

buildPlannerResponse = true;

while(!finishedBuildingResponse)
{
sleep(1.0);
}

ROS_INFO("Planner Is Finished");

requestRender = true;

ROS_INFO("Cleaning Up");

startPlanner = false;
plannerStarted = false;
ROS_INFO("About to Call emit emitProcessPlannerResultsInMainThread();");
emit emitProcessPlannerResultsInMainThread();

plan_grasps_as->setSucceeded(result_);

ROS_INFO("Action ServerCB Finished");
}

void GraspitInterface::startPlannerInMainLoop()
void GraspitInterface::runPlannerInMainThread()
{
ROS_INFO("Inside: runPlannerInMainLoop");
if(mPlanner != NULL)
{
delete mPlanner;
Expand Down Expand Up @@ -669,11 +627,13 @@ void GraspitInterface::startPlannerInMainLoop()
case graspit_interface::Planner::SIM_ANN :
{
mPlanner = new SimAnnPlanner(mHand);
ROS_INFO("Using graspit_interface::Planner::SIM_ANN ");
break;
}
case graspit_interface::Planner::MULTI_THREADED :
{
mPlanner = new GuidedPlanner(mHand);
ROS_INFO("Using graspit_interface::Planner::MULTI_THREADED ");
break;
}
default:
Expand All @@ -687,26 +647,31 @@ void GraspitInterface::startPlannerInMainLoop()
case graspit_interface::SearchEnergy::ENERGY_CONTACT_QUALITY :
{
mPlanner->setEnergyType(ENERGY_CONTACT_QUALITY);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_CONTACT_QUALITY ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_POTENTIAL_QUALITY :
{
mPlanner->setEnergyType(ENERGY_POTENTIAL_QUALITY);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_POTENTIAL_QUALITY ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_CONTACT :
{
mPlanner->setEnergyType(ENERGY_CONTACT);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_CONTACT ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_AUTOGRASP_QUALITY :
{
mPlanner->setEnergyType(ENERGY_AUTOGRASP_QUALITY);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_AUTOGRASP_QUALITY ");
break;
}
case graspit_interface::SearchEnergy::ENERGY_GUIDED_AUTOGRASP :
{
mPlanner->setEnergyType(ENERGY_GUIDED_AUTOGRASP);
ROS_INFO("Using graspit_interface::SearchEnergy::ENERGY_GUIDED_AUTOGRASP ");
break;
}
default:
Expand All @@ -720,11 +685,13 @@ void GraspitInterface::startPlannerInMainLoop()
case graspit_interface::SearchContact::CONTACT_PRESET :
{
mPlanner->setContactType(CONTACT_PRESET);
ROS_INFO("Using graspit_interface::SearchContact::CONTACT_PRESET ");
break;
}
case graspit_interface::SearchContact::CONTACT_LIVE :
{
mPlanner->setContactType(CONTACT_LIVE);
ROS_INFO("Using graspit_interface::SearchContact::CONTACT_LIVE ");
break;
}
default:
Expand All @@ -734,30 +701,36 @@ void GraspitInterface::startPlannerInMainLoop()
}
}

ROS_INFO("Setting Planner Model State");
mPlanner->setModelState(mHandObjectState);
int max_steps = goal.max_steps;
if(max_steps ==0)
{
max_steps = 70000;
}
ROS_INFO("Setting Planner Max Steps %d", max_steps);
mPlanner->setMaxSteps(max_steps);

ROS_INFO("resetting Planner");
mPlanner->resetPlanner();

ROS_INFO("Starting Planner");
mPlanner->startPlanner();
startPlanner = false;
plannerStarted = true;
}

}

void GraspitInterface::buildPlannerResponseInMainLoop()
{
buildPlannerResponse = false;
Hand *mHand = graspitCore->getWorld()->getCurrentHand();
if(mHand == NULL)
{
ROS_INFO("Planning Hand is NULL");
}
void GraspitInterface::processPlannerResultsInMainThread()
{
Hand *mHand = graspitCore->getWorld()->getCurrentHand();
if(mHand == NULL)
{
ROS_INFO("Planning Hand is NULL");
}
GraspableBody *mObject = graspitCore->getWorld()->getGB(0);
if(mObject == NULL)
{
ROS_INFO("Planning Object is NULL");
}

ROS_INFO("Publishing Result");
for(int i = 0; i < mPlanner->getListSize(); i++)
Expand Down Expand Up @@ -810,19 +783,6 @@ void GraspitInterface::buildPlannerResponseInMainLoop()
{
mPlanner->showGrasp(0);
}

finishedBuildingResponse=true;
}

void GraspitInterface::renderInMainLoop()
{
requestRender = false;

ROS_INFO("Rendering");
if(graspitCore->getIVmgr())
{
graspitCore->getIVmgr()->getViewer()->render();
}
}

}

0 comments on commit 926e4eb

Please sign in to comment.