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; }