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