PyObject* py_loadTransferFunction(PyObject* /*self*/, PyObject* args) { static PythonParameterParser tester; std::string path; std::string filename; if (tester.parse(args, path, filename) == -1) { return nullptr; } auto tf = getTF(path, "loadTransferFunction"); if (!tf) { return nullptr; } auto app = InviwoApplication::getPtr(); if (!filesystem::fileExists(filename)) { if (filesystem::fileExists(app->getPath(PathType::TransferFunctions, "/" + filename))) { filename = app->getPath(PathType::TransferFunctions, "/" + filename); } else { std::string msg = "loadTransferFunction() file not found (" + filename + ")"; PyErr_SetString(PyExc_TypeError, msg.c_str()); return nullptr; } } Deserializer deserializer(app, filename); tf->deserialize(deserializer); Py_RETURN_NONE; }
bool ShelfProperties::getBinOriginCB(apc_msgs::GetBinOrigin::Request &req, apc_msgs::GetBinOrigin::Response &res) { geometry_msgs::Point pout; if(!param_loaded) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Parameters not loaded."); return false; } for(std::vector<Bin>::const_iterator it = bin_vec.begin(); it != bin_vec.end(); ++it) { int i = 0; if( (*it).number == req.bin_number) { geometry_msgs::Point pin = (*it).o; if(! getTF(req.frame, pin, pout) ) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Failed to get transform from %s to %s", req.frame.c_str(), shelf_frame.c_str()); return false; } res.point = pout; return true; } i++; } ROS_ERROR("ShelfProperties::getBinOriginCB -> Did not find bin number."); return false; }
PyObject* py_clearTransferfunction(PyObject* /*self*/, PyObject* args) { static PythonParameterParser tester; std::string path; if (tester.parse(args, path) == -1) { return nullptr; } auto tf = getTF(path, "clearTransferfunction"); if (!tf) { return nullptr; } tf->get().clearPoints(); Py_RETURN_NONE; }
PyObject* py_saveTransferFunction(PyObject* /*self*/, PyObject* args) { static PythonParameterParser tester; std::string path; std::string filename; if (tester.parse(args,path,filename) == -1) { return nullptr; } auto tf = getTF(path,"saveTransferFunction"); if (!tf) { return nullptr; } Serializer serializer(filename); tf->serialize(serializer); serializer.writeFile(); Py_RETURN_NONE; }
PyObject* py_addPointTransferFunction(PyObject* /*self*/, PyObject* args) { static PythonParameterParser tester; std::string path; vec2 pos; vec3 color; if (tester.parse(args, path, pos, color) == -1) { return nullptr; } auto tf = getTF(path, "addPointTransferFunction"); if (!tf) { return nullptr; } tf->get().addPoint(pos, vec4(color, pos.y)); tf->setModified(true); Py_RETURN_NONE; }
int main(int argc, char **argv) { ros::init(argc, argv, "print_state_node"); ros::NodeHandle n; std::string robot_frame = "base_link"; if (argc > 1) { robot_frame = argv[1]; } ROS_INFO_STREAM("Frame id is: " << robot_frame ); ros::ServiceClient srv_get_joints_ = n.serviceClient<apc_msgs::ReturnJointStates>("return_joint_states"); if( !srv_get_joints_.waitForExistence(ros::Duration(1.0) ) ) ROS_ERROR("Joint state listener not running! Please run \n rosrun apc_robot joint_states_listener.py"); listenerPtr = new tf::TransformListener(); std::string r_controller_name = "r_arm_controller_loose"; std::string l_controller_name = "l_arm_controller_loose"; std::string r_frame = "r_gripper_tool_frame"; std::string l_frame = "l_gripper_tool_frame"; joints_right.request.name.resize(0); joints_right.request.name.push_back( "r_shoulder_pan_joint" ); joints_right.request.name.push_back( "r_shoulder_lift_joint" ); joints_right.request.name.push_back( "r_upper_arm_roll_joint" ); joints_right.request.name.push_back( "r_elbow_flex_joint" ); joints_right.request.name.push_back( "r_forearm_roll_joint" ); joints_right.request.name.push_back( "r_wrist_flex_joint" ); joints_right.request.name.push_back( "r_wrist_roll_joint" ); joints_left.request.name.resize(0); joints_left.request.name.push_back( "l_shoulder_pan_joint" ); joints_left.request.name.push_back( "l_shoulder_lift_joint" ); joints_left.request.name.push_back( "l_upper_arm_roll_joint" ); joints_left.request.name.push_back( "l_elbow_flex_joint" ); joints_left.request.name.push_back( "l_forearm_roll_joint" ); joints_left.request.name.push_back( "l_wrist_flex_joint" ); joints_left.request.name.push_back( "l_wrist_roll_joint" ); tf::StampedTransform transform; int Hz = 50; ros::Rate loopRate(Hz); // Wait for the listener to get the first message if(! listenerPtr->waitForTransform(l_frame, robot_frame, ros::Time(0), ros::Duration(3.0)) ) ROS_ERROR("Could not fprintTFind source->target transform!"); if(! listenerPtr->waitForTransform(r_frame, robot_frame, ros::Time(0), ros::Duration(3.0)) ) ROS_ERROR("Could not fprintTFind source->target transform!"); std::string tmp; bool done = false; while(ros::ok() && !done) { std::cout<<"Press [enter] to get joint state (q to quit):"; std::getline(std::cin, tmp); if(tmp == "q") { done=true; } else { if( getTF(r_frame, robot_frame, transform) ) printTF(transform, RIGHT); if( getTF(l_frame, robot_frame, transform) ) printTF(transform, LEFT); } std::cout<<"-----------------------\n"; if(srv_get_joints_.call(joints_left)) { std::cout<<"left: = ["; for(int k = 0;k<7;k++) { std::cout<<joints_left.response.position[k]; if(k<6) std::cout<<","; else std::cout<<"];\n"; } } if(srv_get_joints_.call(joints_right)) { std::cout<<"right: = ["; for(int k = 0;k<7;k++) { std::cout<<joints_right.response.position[k]; if(k<6) std::cout<<","; else std::cout<<"];\n"; } } std::cout<<"\n###############################\n"; ROS_INFO(""); } ROS_INFO("Shutting down ... "); ros::shutdown(); return 0; }
bool ShelfProperties::publishBinCenterMarker(std::string target_frame) { visualization_msgs::MarkerArray markerArray; apc_msgs::GetBinInfo msg_getBinInfo; geometry_msgs::Point center; geometry_msgs::Point p; // 12 bins present and 12 markers in the array denoting center points markerArray.markers.resize(12); for ( int i = 0; i < 12; i++) { // Marker identifiers markerArray.markers[i].header.frame_id = target_frame; markerArray.markers[i].header.stamp = ros::Time(); markerArray.markers[i].ns = "shelf_properties"; markerArray.markers[i].id = i; // Marker shape markerArray.markers[i].type = visualization_msgs::Marker::SPHERE; markerArray.markers[i].action = visualization_msgs::Marker::ADD; // Get bin details int bin = i+1; std::string frame = target_frame; for(std::vector<Bin>::const_iterator it = bin_vec.begin(); it != bin_vec.end(); ++it) { if( (*it).number == bin) { double w = (*it).width; double h = (*it).height; geometry_msgs::Point center = (*it).o; center.y -= w/2; center.z += h/2; getTF(frame, center, p); } } // Marker position and orientation markerArray.markers[i].pose.position.x = p.x; markerArray.markers[i].pose.position.y = p.y; markerArray.markers[i].pose.position.z = p.z; markerArray.markers[i].pose.orientation.x = 0.0; markerArray.markers[i].pose.orientation.y = 0.0; markerArray.markers[i].pose.orientation.z = 0.0; markerArray.markers[i].pose.orientation.w = 1.0; // Marker size and color markerArray.markers[i].scale.x = 0.05; markerArray.markers[i].scale.y = 0.05; markerArray.markers[i].scale.z = 0.05; markerArray.markers[i].color.a = 0.5; markerArray.markers[i].color.r = 1.0; markerArray.markers[i].color.g = 0.0; markerArray.markers[i].color.b = 0.0; } pub_marker_bin_center_.publish(markerArray); }
bool ShelfProperties::getBinInfoCB(apc_msgs::GetBinInfo::Request &req, apc_msgs::GetBinInfo::Response &res) { geometry_msgs::Point pout, oout, aout, bout, cout; if(!param_loaded) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Parameters not loaded."); return false; } for(std::vector<Bin>::const_iterator it = bin_vec.begin(); it != bin_vec.end(); ++it) { int i = 0; if( (*it).number == req.bin_number) { double w = (*it).width; double h = (*it).height; geometry_msgs::Point center = (*it).o; geometry_msgs::Point o = (*it).o; geometry_msgs::Point a = (*it).a; geometry_msgs::Point b = (*it).b; geometry_msgs::Point c = (*it).c; center.y -= w/2; center.z += h/2; if(! getTF(req.frame, center, pout) ) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Failed to get transform from %s to %s", req.frame.c_str(), shelf_frame.c_str()); return false; } if(! getTF(req.frame, o, oout) ) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Failed to get transform from %s to %s", req.frame.c_str(), shelf_frame.c_str()); return false; } if(! getTF(req.frame, a, aout) ) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Failed to get transform from %s to %s", req.frame.c_str(), shelf_frame.c_str()); return false; } if(! getTF(req.frame, b, bout) ) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Failed to get transform from %s to %s", req.frame.c_str(), shelf_frame.c_str()); return false; } if(! getTF(req.frame, c, cout) ) { ROS_ERROR("ShelfProperties::getBinOriginCB -> Failed to get transform from %s to %s", req.frame.c_str(), shelf_frame.c_str()); return false; } res.center = pout; res.o = oout; res.a = aout; res.b = bout; res.c = cout; res.height = h; // TODO verify res.width = w; return true; } i++; } ROS_ERROR("ShelfProperties::getBinOriginCB -> Did not find bin number."); return false; }