void BuilderAppCoordinator::closeProject()
{
openfluid::base::ProjectManager::getInstance()->close();
setHomeModule();
setState(*getHomeState());
}
/**
* Sends a sensor_msgs/JointState message to a topic in order to set the joint state
* to the fixed position where it can grasp a cube. Helper functions are available
* for cubes spawned on jaco_on_table at a certain pose.
*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "set_joint_state_cube_publisher");
ros::NodeHandle n("");
bool setHome=false;
bool skipFingers=false;
if (argc > 1)
{
for (int i=1; i<argc; ++i)
{
std::string arg=argv[i];
if (arg=="--home")
{
ROS_INFO("Setting jaco to home position.");
setHome=true;
}
if (arg=="--no-fingers")
{
ROS_INFO("Setting jaco to home position.");
skipFingers=true;
}
}
}
JacoJointManager joints;
std::string jointStateTopic(JOINT_STATES_TOPIC);
std::string jointStatePublishTopic(JOINT_CONTROL_TOPIC);
ROS_INFO_STREAM("Publishing on " << jointStatePublishTopic);
ros::Publisher pub = n.advertise<sensor_msgs::JointState>(jointStatePublishTopic, 1000, true);
sensor_msgs::JointState currPosState;
// in case we skip the fingers, we'll need the actual current state of the robot
// to initialize new joint states later on.
if (skipFingers)
{
std::vector<std::string> jointNames;
joints.getJointNames(jointNames,true);
// get the current state, as other joints of jaco have to remain the same
sensor_msgs::JointState _currPosState = getCurrentJointState(jointStateTopic, n);
std::vector<int> idx;
if ((joints.getJointIndices(_currPosState.name, idx) != 0) || (idx.size()!=jointNames.size()))
{
ROS_ERROR("Current state does not have all arm and finger joint names.");
return 1;
}
currPosState.position.resize(idx.size());
for (int i=0; i<idx.size(); ++i) currPosState.position[i] = _currPosState.position[idx[i]];
}
while (pub.getNumSubscribers() == 0)
{
ROS_INFO("SetArmToCubePose: waiting for subscribers...");
ros::Duration(1).sleep();
}
if (!setHome)
{
sensor_msgs::JointState grasp_state;
sensor_msgs::JointState pre_grasp_state;
getJointStateCube1(joints,pre_grasp_state, grasp_state);
if (skipFingers)
{
resetFingers(pre_grasp_state, currPosState);
resetFingers(grasp_state, currPosState);
}
ROS_INFO_STREAM("Publishing pre-grasp state" << pre_grasp_state);
pub.publish(pre_grasp_state);
// Wait for the arm to arrive. This could be improved by waiting
// for the exact state to be reached, but it's only a test at this
// stage anyway.
ros::Duration(3).sleep();
ROS_INFO_STREAM("Publishing grasp state" << grasp_state);
pub.publish(grasp_state);
}
else
{
sensor_msgs::JointState home_state=getHomeState(joints);
if (skipFingers) resetFingers(home_state, currPosState);
ROS_INFO_STREAM("Publishing grasp state" << home_state);
pub.publish(home_state);
}
// wait to allow that the last message arrives before quitting node
ros::Duration(1).sleep();
return 0;
}
