You can subscribe to this list here.
2008 |
Jan
|
Feb
|
Mar
|
Apr
|
May
|
Jun
|
Jul
|
Aug
|
Sep
|
Oct
|
Nov
(4) |
Dec
|
---|---|---|---|---|---|---|---|---|---|---|---|---|
2009 |
Jan
|
Feb
|
Mar
(7) |
Apr
|
May
|
Jun
(40) |
Jul
(28) |
Aug
(51) |
Sep
(63) |
Oct
(42) |
Nov
(47) |
Dec
(19) |
2010 |
Jan
(5) |
Feb
(12) |
Mar
(23) |
Apr
(102) |
May
(2) |
Jun
(11) |
Jul
(32) |
Aug
(51) |
Sep
(64) |
Oct
(40) |
Nov
(86) |
Dec
(29) |
2011 |
Jan
(1) |
Feb
(10) |
Mar
(5) |
Apr
|
May
(3) |
Jun
(5) |
Jul
(3) |
Aug
(2) |
Sep
(11) |
Oct
(5) |
Nov
|
Dec
(1) |
2012 |
Jan
(6) |
Feb
(8) |
Mar
(2) |
Apr
(2) |
May
|
Jun
(2) |
Jul
(3) |
Aug
(2) |
Sep
|
Oct
|
Nov
(1) |
Dec
|
From: <tum...@li...> - 2012-01-27 18:36:50
|
Revision: 983 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=983&view=rev Author: moesenle Date: 2012-01-27 18:36:44 +0000 (Fri, 27 Jan 2012) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Fri Jan 27 19:36:01 2012 +0100 commit 260b7704746b8185e64e3ccb0284ec3054176ff2 Author: Dejan Pangercic <dej...@gm...> Date: Fri Jan 27 19:34:40 2012 +0100 zbar compiled w/o video support, cpp example of zbar client commit ff0e5736c6a7962b01db874e5e794c7fff131b4e Author: Dejan Pangercic <dej...@cs...> Date: Mon Aug 30 11:25:18 2010 +0200 public export git-commit Modified Paths: -------------- 3rdparty/zbar/Makefile Modified: 3rdparty/zbar/Makefile =================================================================== --- 3rdparty/zbar/Makefile 2012-01-27 09:52:17 UTC (rev 982) +++ 3rdparty/zbar/Makefile 2012-01-27 18:36:44 UTC (rev 983) @@ -9,7 +9,9 @@ include $(shell rospack find mk)/download_unpack_build.mk installed: $(SOURCE_DIR)/unpacked Makefile - cd $(SOURCE_DIR) && ./configure --prefix=$(INSTALL_DIR) --without-python && make && make install + cd $(SOURCE_DIR) && ./configure --prefix=$(INSTALL_DIR) --without-python --disable-video + make -C $(SOURCE_DIR) + make -C $(SOURCE_DIR) install touch installed clean: wipe This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-12-14 16:29:26
|
Revision: 980 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=980&view=rev Author: moesenle Date: 2011-12-14 16:29:20 +0000 (Wed, 14 Dec 2011) Log Message: ----------- Author: Thomas Ruehr <ru...@in...> Date: Wed Dec 14 17:25:57 2011 +0100 Helper node to determine a base pose that makes executing cartesian trajectories with the arms feasible. The action interface should be straight forward, the input is a vector of positions in arbitrary frame together with a vector of arm indices, 0 for right and 1 for left arm. The result will be a pose in base_link that makes the trajectories for both arms reachable. Currently only poses are generated that do not bring the base into collision with base_laser obstacles. This and other features will be pulled out as parameters in the future. Use with caution, some bugs might be hidden here and there ;) Now respects and stays out of map obstacles and unknown space. git-commit Modified Paths: -------------- highlevel/find_base_pose/Makefile highlevel/find_base_pose/manifest.xml highlevel/find_base_pose/src/find_base_pose.cpp highlevel/find_base_pose/src/test_client.cpp Modified: highlevel/find_base_pose/Makefile =================================================================== --- highlevel/find_base_pose/Makefile 2011-11-02 10:08:47 UTC (rev 979) +++ highlevel/find_base_pose/Makefile 2011-12-14 16:29:20 UTC (rev 980) @@ -1 +1 @@ -include $(shell rospack find mk)/cmake.mk +include $(shell rospack find mk)/cmake.mk \ No newline at end of file Modified: highlevel/find_base_pose/manifest.xml =================================================================== --- highlevel/find_base_pose/manifest.xml 2011-11-02 10:08:47 UTC (rev 979) +++ highlevel/find_base_pose/manifest.xml 2011-12-14 16:29:20 UTC (rev 980) @@ -13,6 +13,7 @@ <depend package="kinematics_msgs" /> <depend package="tf" /> <depend package="move_base_msgs" /> + <depend package="nav_msgs" /> <depend package="geometry_msgs" /> <depend package="kinematics_base" /> <depend package="std_srvs" /> Modified: highlevel/find_base_pose/src/find_base_pose.cpp =================================================================== --- highlevel/find_base_pose/src/find_base_pose.cpp 2011-11-02 10:08:47 UTC (rev 979) +++ highlevel/find_base_pose/src/find_base_pose.cpp 2011-12-14 16:29:20 UTC (rev 980) @@ -1,4 +1,3 @@ - /* * Copyright (c) 2011, Thomas Ruehr <ru...@cs...> * All rights reserved. @@ -56,7 +55,193 @@ pr2_arm_kinematics::PR2ArmIKSolver *solver; +//! exlude unknown and occupied space from map +#include <nav_msgs/OccupancyGrid.h> +bool we_have_a_map = false; +bool map_processed = false; + +nav_msgs::OccupancyGrid map, map_inscribed, map_circumscribed; +nav_msgs::OccupancyGrid coarsemap; + +float resolution_ = 0; +int size_x_ = 0; +int size_y_ = 0; + +float origin_x_ = 0; +float origin_y_ = 0; + +void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) +{ + wx = origin_x_ + (mx + 0.5) * resolution_; + wy = origin_y_ + (my + 0.5) * resolution_; +} + +bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) +{ + if (wx < origin_x_ || wy < origin_y_) + return false; + + mx = (int) ((wx - origin_x_) / resolution_); + my = (int) ((wy - origin_y_) / resolution_); + + if (mx < size_x_ && my < size_y_) + return true; + + return false; +} + +void worldToMapNoBounds(double wx, double wy, int& mx, int& my) +{ + mx = (int) ((wx - origin_x_) / resolution_); + my = (int) ((wy - origin_y_) / resolution_); +} + +ros::Publisher occ_pub; + +void mapCallback(const nav_msgs::OccupancyGrid& msg) +{ + if (we_have_a_map) + return; + + we_have_a_map = true; + + map = msg; + map_inscribed = msg; + + resolution_ = map.info.resolution; + size_x_ = map.info.width; + size_y_ = map.info.height; + + origin_x_ = map.info.origin.position.x; + origin_y_ = map.info.origin.position.y; + + ROS_INFO("got map, processing "); + + double inscribing_radius = .35; + int inscr_rad = inscribing_radius / map.info.resolution; + + for (int my = 0; my < size_y_; my++) + { + if ((my + 1) % (int)(size_x_ / 20) == 0) + { + ROS_INFO("processing %i/%i", my, size_y_); + } + for (int mx = 0; mx < size_x_; mx++) + { + bool found = false; + if (map_inscribed.data[mx + my * size_x_] ==0) + for (int ax = -inscr_rad; !found && (ax < inscr_rad); ax++) + for (int ay = -inscr_rad; !found && (ay < inscr_rad); ay++) + { + if ((ax * ax + ay * ay < inscr_rad * inscr_rad) && + (mx + ax > 0) && (mx + ax < size_x_) && + (my + ay > 0) && (my + ay < size_y_) && + (map.data[mx + ax + (my + ay) * size_x_] != 0)) + { + found = true; + map_inscribed.data[mx + my * size_x_] = 100; + } + } + } + } + + inscribing_radius = tf::Vector3(.35,.35,0).length(); + ROS_INFO("circumscribed circle radius %f", inscribing_radius); + inscr_rad = inscribing_radius / map.info.resolution; + + map_circumscribed = map_inscribed; + + for (int my = 0; my < size_y_; my++) + { + if ((my + 1) % (int)(size_x_ / 20) == 0) + { + ROS_INFO("processing %i/%i", my, size_y_); + } + for (int mx = 0; mx < size_x_; mx++) + { + bool found = false; + if (map_circumscribed.data[mx + my * size_x_] ==0) + for (int ax = -inscr_rad; !found && (ax < inscr_rad); ax++) + for (int ay = -inscr_rad; !found && (ay < inscr_rad); ay++) + { + if ((ax * ax + ay * ay < inscr_rad * inscr_rad) && + (mx + ax > 0) && (mx + ax < size_x_) && + (my + ay > 0) && (my + ay < size_y_) && + (map.data[mx + ax + (my + ay) * size_x_] != 0)) + { + found = true; + map_circumscribed.data[mx + my * size_x_] = 100; + } + } + } + } + + map_processed = true; + + ROS_INFO("Processing done"); + + ros::NodeHandle node_handle; + occ_pub = node_handle.advertise<nav_msgs::OccupancyGrid>( "occ_map", 1000, true ); + occ_pub.publish(map_inscribed); + + ROS_INFO("Publishing done"); +} + +//will take a pose in map coordinates and say if its free given the pr2s footprint +bool mapFreeCoarse(tf::Stamped<tf::Pose> absPose) +{ + ros::Rate rt(2); + while (!map_processed) + { + rt.sleep(); + ROS_INFO("Waiting for map to be published on /map"); + } + //bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) + unsigned int mx, my; + worldToMap(absPose.getOrigin().x(), absPose.getOrigin().y(), mx,my); + if (map_inscribed.data[mx + my * map.info.width] == 0) + return true; + return false; +} + +bool mapFreeCoarseCircumscribed(tf::Stamped<tf::Pose> absPose) +{ + ros::Rate rt(2); + while (!map_processed) + { + rt.sleep(); + ROS_INFO("Waiting for map to be published on /map"); + } + //bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) + unsigned int mx, my; + worldToMap(absPose.getOrigin().x(), absPose.getOrigin().y(), mx,my); + if (map_circumscribed.data[mx + my * map.info.width] == 0) + return true; + return false; +} + +bool mapFreeFine(tf::Stamped<tf::Pose> absPose) +{ + ros::Rate rt(2); + while (!map_processed) + { + rt.sleep(); + ROS_INFO("Waiting for map to be published on /map"); + } + //bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) + + unsigned int mx, my; + worldToMap(absPose.getOrigin().x(), absPose.getOrigin().y(), mx,my); + if (map.data[mx + my * map.info.width] == 0) + return true; + return false; +} + + +//! end: exlude unknown and occupied space from map + + class FindBasePoseAction { protected: @@ -220,9 +405,9 @@ marker.type = visualization_msgs::Marker::ARROW; marker.action = visualization_msgs::Marker::ADD; marker.pose = pose.pose; - marker.scale.x = 0.2; - marker.scale.y = 0.7; - marker.scale.z = 0.7; + marker.scale.x = 0.02; + marker.scale.y = 0.07; + marker.scale.z = 0.07; marker.color.a = 1; marker.color.r = colr; marker.color.g = colg; @@ -409,7 +594,6 @@ marker.color.r = colr; marker.color.g = colg; marker.color.b = colb; - float si = si_; std::vector<btVector3>::iterator it; for (it = pts.begin(); it != pts.end(); ++it) @@ -453,35 +637,55 @@ weHaveScan = true; } - bool collisionFree(float relativePose[]) + + //bool collisionFree(tf::Stamped<tf::Pose> relativePose) + + //check against map collision + bool collisionFreeCoarse(btTransform relativePose) { - ros::Rate rate(10); - while (!weHaveScan) - { - rate.sleep(); - ros::spinOnce(); + + tf::Stamped<tf::Pose> relPose; + relPose.setOrigin(relativePose.getOrigin()); + relPose.setRotation(relativePose.getRotation()); + relPose.frame_id_ = "/base_link"; + tf::Stamped<tf::Pose> absPose = getPoseIn("/map", relPose); + + if (!mapFreeCoarse(absPose)) + return false; + + return true; + } + + //check against map collision + bool collisionFreeFine(btTransform relativePose) + { + + tf::Stamped<tf::Pose> relPose; + relPose.setOrigin(relativePose.getOrigin()); + relPose.setRotation(relativePose.getRotation()); + relPose.frame_id_ = "/base_link"; + + tf::Stamped<tf::Pose> absPose = getPoseIn("/map", relPose); + if (mapFreeCoarseCircumscribed(absPose)) + return true; + + for (double ax = -.35; ax <= .35; ax += .1) + for (double ay = -.35; ay <= .35; ay += .1) { + tf::Stamped<tf::Pose> act = relPose; + act.getOrigin() += tf::Vector3(ax,ay,0); + + tf::Stamped<tf::Pose> absPose = getPoseIn("/map", act); + if (!mapFreeCoarse(absPose)) + return false; } - scan_mutex.lock(); - //ROS_INFO("SCAN POINTS : %i",numScanPoints); - bool good = true; - float padding = 0.05; - for (size_t k = 0; good && (k < numScanPoints); k += 1) - { - //float x = scanPoints[k][0] - relativePose[0] + 0.275; - //float y = scanPoints[k][1] - relativePose[1]; - float x = scanPoints[k].x() - relativePose[0] + 0.275; - float y = scanPoints[k].y() - relativePose[1]; - if ((x < .325 + padding) && (x > -.325 - padding) && (y < .325 + padding) && (y > -.325 - padding)) - { - //ROS_INFO("POINT %f %f", scanPoints[k][0] , scanPoints[k][1]); - good = false; - } - } - scan_mutex.unlock(); - return good; + + return true; } - //bool collisionFree(tf::Stamped<tf::Pose> relativePose) + //todo: speed this up ! 10x should be no problem, with a real algorithm ;) + // other idea: first check circumscribed circle against points, then incircle, + // only check polygon for points inside circumscribed and outside of inscribed circle + bool collisionFree(btTransform relativePose) { ros::Rate rate(10); @@ -491,15 +695,31 @@ ros::spinOnce(); } scan_mutex.lock(); + + tf::Stamped<tf::Pose> relPose; + relPose.setOrigin(relativePose.getOrigin()); + relPose.setRotation(relativePose.getRotation()); + relPose.frame_id_ = "/base_link"; + tf::Stamped<tf::Pose> absPose = getPoseIn("/map", relPose); + + if (!mapFreeCoarse(absPose)) + return false; + //ROS_INFO("SCAN POINTS : %i",numScanPoints); bool good = true; + bool happy = true; + bool indifferent = true; float padding = 0.05; + float inscribed = .325; + float circumscribed = sqrt(inscribed * inscribed + inscribed * inscribed); for (size_t k = 0; good && (k < numScanPoints); k += 1) { - - //float x = scanPoints[k].x() - relativePose.getOrigin().x() + 0.275; - //float y = scanPoints[k].y() - relativePose.getOrigin().y(); btVector3 in = scanPoints[k]; + float dist = (in - relativePose.getOrigin()).length(); + if (dist < inscribed) + happy = false; + if (dist < circumscribed) + indifferent = false; btVector3 transformed = relativePose.invXform(in); //btVector3 transformed = in * relativePose; if ((transformed.x() < .325 + padding) && (transformed.x() > -.325 - padding) && (transformed.y() < .325 + padding) && (transformed.y() > -.325 - padding)) @@ -507,6 +727,10 @@ //ROS_INFO("POINT %f %f", scanPoints[k][0] , scanPoints[k][1]); good = false; } + if (good && !happy) + ROS_ERROR("Good but not happy? Point lies in in incircle but not in box, how can that be ?"); + if (!good && indifferent) + ROS_ERROR("Not good but indifferent? Point lies in footprint box but not inside circumscribed circle, how can that be?"); } scan_mutex.unlock(); return good; @@ -524,14 +748,6 @@ return ret; } - /*tf::Stamped<tf::Pose> getPoseIn(const char target_frame[], tf::Stamped<tf::Pose>src) - { - tf::Stamped<tf::Pose> transform; - listener_->waitForTransform(src.frame_id_, target_frame,ros::Time(0), ros::Duration(30.0)); - listener_->transformPose(target_frame, src, transform); - return transform; - }*/ - tf::Stamped<tf::Pose> getPoseIn(const char target_frame[], tf::Stamped<tf::Pose>src) { @@ -677,6 +893,9 @@ else ik_service_call = kinemal->getPositionIK(ik_request,ik_response); } + + //! TODO: generally we should be able to interface ik on a lower level + //bool ik_service_call = true; //Eigen::eigen2_Transform3d pose_eigen; //KDL::Frame pose_kdl; @@ -761,7 +980,7 @@ //}(-1.008435,-1.813615)-(1.591565,0.786385) // max of mins and vice versa gives the overlap of approx inverse capability maps - ROS_INFO("GOAL TARGETS SIZE: %zu", target_poses_transformed.size()); + //ROS_INFO("GOAL TARGETS SIZE: %zu", target_poses_transformed.size()); for (size_t i=0; i < target_poses_transformed.size(); ++i) { // take 1.5m for approx upper bound of arms reach @@ -769,11 +988,11 @@ maxx = std::min(target_poses_transformed[i].getOrigin().x()-bbox[arm[i].data][0].x(),maxx); miny = std::max(target_poses_transformed[i].getOrigin().y()-bbox[arm[i].data][1].y(),miny); maxy = std::min(target_poses_transformed[i].getOrigin().y()-bbox[arm[i].data][0].y(),maxy); - ROS_INFO("%zu BOUNDING BOX : (%f,%f)-(%f,%f) size : %f %f ",i, minx,miny, maxx, maxy, maxx - minx, maxy - miny); + //ROS_INFO("%zu BOUNDING BOX : (%f,%f)-(%f,%f) size : %f %f ",i, minx,miny, maxx, maxy, maxx - minx, maxy - miny); //pubBaseMarker(goalInBaseMsg[i].pose.position.x,goalInBaseMsg[i].pose.position.y,goalInBaseMsg[i].pose.position.z,1,0,0); } - ROS_INFO("BOUNDING BOX : (%f,%f)-(%f,%f) size : %f %f ", minx,miny, maxx, maxy, maxx - minx, maxy - miny); + //ROS_INFO("BOUNDING BOX : (%f,%f)-(%f,%f) size : %f %f ", minx,miny, maxx, maxy, maxx - minx, maxy - miny); //discretize on a grid around 0,0 double discretisation = 0.075; @@ -866,7 +1085,7 @@ std::vector<geometry_msgs::PoseStamped> goalInBaseMsg; goalInBaseMsg.resize(goal->target_poses.size()); - double reach = 1.3; + //double reach = 1.3; for (size_t i=0; i < goal->target_poses.size(); ++i) { @@ -877,6 +1096,10 @@ goalInBase[i] = tool2wrist(getPoseIn("base_link", act)); poseStampedTFToMsg(goalInBase[i], goalInBaseMsg[i]); + tf::Stamped<tf::Pose> inBase = getPoseIn("base_link", act); + ROS_INFO("TOOL in base: %f %f %f %f %f %f %f", inBase.getOrigin().x(), inBase.getOrigin().y(), inBase.getOrigin().z(), + inBase.getRotation().x(), inBase.getRotation().y(), inBase.getRotation().z(), inBase.getRotation().w()); + for (int k = 0 ; k < 5 ; k++) { //pubBaseMarkerArrow(goal->target_poses[i],goal->arm[i].data,1-goal->arm[i].data,0); @@ -888,8 +1111,8 @@ } ROS_INFO("FRAME: %s", goalInBaseMsg[i].header.frame_id.c_str()); - ROS_INFO("POSE: %f %f %f", goalInBaseMsg[i].pose.position.x, goalInBaseMsg[i].pose.position.y, goalInBaseMsg[i].pose.position.z); - ROS_INFO("ROT: %f %f %f %f", goalInBaseMsg[i].pose.orientation.x, goalInBaseMsg[i].pose.orientation.y, goalInBaseMsg[i].pose.orientation.z, goalInBaseMsg[i].pose.orientation.w); + ROS_INFO("WRIST POSE: %f %f %f", goalInBaseMsg[i].pose.position.x, goalInBaseMsg[i].pose.position.y, goalInBaseMsg[i].pose.position.z); + ROS_INFO("WRIST ROT: %f %f %f %f", goalInBaseMsg[i].pose.orientation.x, goalInBaseMsg[i].pose.orientation.y, goalInBaseMsg[i].pose.orientation.z, goalInBaseMsg[i].pose.orientation.w); } std::vector<tf::Pose> search_poses; @@ -916,7 +1139,7 @@ - ROS_INFO("SIZE Of searchspace : %i .. sorting", (int)search_poses.size()); + //ROS_INFO("SIZE Of searchspace : %i .. sorting", (int)search_poses.size()); std::sort(search_poses.begin(), search_poses.end(), pose_compare); @@ -1005,13 +1228,21 @@ bool found = false; //while (!found) - int counter = 0; float mindist = 10000; + + int cnt = 1; + for (it = search_poses.begin(); (it!=search_poses.end()) && (!found); ++it) //for (it = search_poses.begin(); it!=search_poses.end(); ++it) { + if (cnt % 50 == 0) + ROS_INFO("Cnt %i", cnt); + + if (!collisionFreeCoarse(*it)) + continue; + //double x_add = it->getOrigin().x(); //double y_add = it->getOrigin().y(); @@ -1043,8 +1274,10 @@ //ROS_INFO("x %f y %f d %f good %i", x_add, y_add, a.length(), good); - if (good) - if (collisionFree(*it)) + if (good) // collision checking seems to take longer than checking ik, this may differ for long trajectories! todo: check this first, then decide order + // apart from that: todo: speed up collision checking, + if (collisionFree(*it) && collisionFreeFine(*it)) + //if (collisionFree(*it) && collisionFreeFine(*it)) { if (!found) { @@ -1131,6 +1364,9 @@ ros::param::set("/find_base_pose/root_name", "torso_lift_link"); ros::param::set("/find_base_pose/tip_name", "r_wrist_roll_link"); + ros::NodeHandle nh_; + ros::Subscriber subMap = nh_.subscribe("map", 10, mapCallback); + ros::Rate rt(10); while (!(ros::param::has("/find_base_pose/root_name") && ros::param::has("/find_base_pose/tip_name") )) @@ -1170,3 +1406,6 @@ return 0; } + + +// failed goal (in base): 0.125900 0.564599 1.007231 -0.057600 0.007678 0.132418 0.989489 (time 1.000000) Modified: highlevel/find_base_pose/src/test_client.cpp =================================================================== --- highlevel/find_base_pose/src/test_client.cpp 2011-11-02 10:08:47 UTC (rev 979) +++ highlevel/find_base_pose/src/test_client.cpp 2011-12-14 16:29:20 UTC (rev 980) @@ -133,12 +133,13 @@ } - test_fridge(ac); - ROS_INFO("Waiting for action server to start."); // wait for the action server to start ac.waitForServer(); //will wait for infinite time + test_fridge(ac); + + ac.sendGoal(goal); //wait for the action to return This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-10-25 10:53:23
|
Revision: 978 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=978&view=rev Author: moesenle Date: 2011-10-25 10:53:17 +0000 (Tue, 25 Oct 2011) Log Message: ----------- Author: Moritz Tenorth <te...@cs...> Date: Tue Oct 25 12:52:42 2011 +0200 commit 15288c970c66e1c39b87ec6c88e06dda4e736479 Author: Moritz Tenorth <te...@cs...> Date: Tue Aug 16 07:34:53 2011 +0200 updated rosjava reference commit 9d7c101545836aa841099a28e49f69da6d1bff98 Author: Moritz Tenorth <te...@cs...> Date: Tue Feb 15 13:42:35 2011 +0100 public export git-commit Modified Paths: -------------- knowledge/mod_dialog/manifest.xml Modified: knowledge/mod_dialog/manifest.xml =================================================================== --- knowledge/mod_dialog/manifest.xml 2011-10-25 10:53:09 UTC (rev 977) +++ knowledge/mod_dialog/manifest.xml 2011-10-25 10:53:17 UTC (rev 978) @@ -11,7 +11,7 @@ <url>http://ros.org/wiki/mod_dialog</url> <depend package="mod_vis"/> - <depend package="rosjava"/> + <depend package="rosjava_jni"/> <depend package="std_msgs"/> <depend package="mary_tts"/> <depend package="comp_ehow"/> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-10-25 10:53:18
|
Revision: 977 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=977&view=rev Author: moesenle Date: 2011-10-25 10:53:09 +0000 (Tue, 25 Oct 2011) Log Message: ----------- Author: Moritz Tenorth <te...@cs...> Date: Tue Oct 25 12:52:42 2011 +0200 commit 15288c970c66e1c39b87ec6c88e06dda4e736479 Author: Moritz Tenorth <te...@cs...> Date: Tue Aug 16 07:34:53 2011 +0200 updated rosjava reference commit 283a6f3cb5a56830b9f4c866c4dc8dae0183911c Author: Moritz Tenorth <te...@cs...> Date: Wed Mar 2 16:27:05 2011 +0100 updated knowrob tutorial to work with the new rosjava version git-commit Modified Paths: -------------- knowledge/knowrob_tutorial/manifest.xml Modified: knowledge/knowrob_tutorial/manifest.xml =================================================================== --- knowledge/knowrob_tutorial/manifest.xml 2011-10-25 10:53:01 UTC (rev 976) +++ knowledge/knowrob_tutorial/manifest.xml 2011-10-25 10:53:09 UTC (rev 977) @@ -10,7 +10,7 @@ <url>http://ros.org/wiki/knowrob_tutorial</url> <depend package="mod_vis"/> <depend package="json_prolog"/> - <depend package="rosjava"/> + <depend package="rosjava_jni"/> <depend package="ias_semantic_map"/> <depend package="tabletop_object_detector"/> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-10-25 10:53:08
|
Revision: 976 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=976&view=rev Author: moesenle Date: 2011-10-25 10:53:01 +0000 (Tue, 25 Oct 2011) Log Message: ----------- Author: Moritz Tenorth <te...@cs...> Date: Tue Oct 25 12:52:42 2011 +0200 commit 15288c970c66e1c39b87ec6c88e06dda4e736479 Author: Moritz Tenorth <te...@cs...> Date: Tue Aug 16 07:34:53 2011 +0200 updated rosjava reference commit 3e78a6ee5b04440742f183421be94c0357452599 Author: Moritz Tenorth <te...@cs...> Date: Thu Feb 10 15:39:10 2011 +0100 public export git-commit Modified Paths: -------------- knowledge/comp_ehow/manifest.xml Modified: knowledge/comp_ehow/manifest.xml =================================================================== --- knowledge/comp_ehow/manifest.xml 2011-10-11 07:00:11 UTC (rev 975) +++ knowledge/comp_ehow/manifest.xml 2011-10-25 10:53:01 UTC (rev 976) @@ -15,7 +15,7 @@ <depend package="ias_knowledge_base"/> <depend package="opencyc"/> <depend package="mod_vis"/> - <depend package="rosjava"/> + <depend package="rosjava_jni"/> <depend package="comp_cop"/> <!-- TODO MT: depend only because of httpclient, refactor this! --> <export> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-10-11 07:00:17
|
Revision: 975 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=975&view=rev Author: moesenle Date: 2011-10-11 07:00:11 +0000 (Tue, 11 Oct 2011) Log Message: ----------- Author: Moritz Tenorth <te...@cs...> Date: Tue Oct 11 08:59:40 2011 +0200 commit 682c8727d9299c33916b24e59637134efb8cce85 Author: Moritz Tenorth <te...@cs...> Date: Tue Oct 11 08:58:55 2011 +0200 fixing mary_tts: reference to rosjava_jni commit 3aaed8f2cbb22ddfb1cecf84711c7f23fb74370f Author: Moritz Tenorth <te...@cs...> Date: Wed Feb 9 14:41:55 2011 +0100 public export git-commit Modified Paths: -------------- knowledge/3rdparty/mary_tts/manifest.xml Modified: knowledge/3rdparty/mary_tts/manifest.xml =================================================================== --- knowledge/3rdparty/mary_tts/manifest.xml 2011-10-04 18:24:00 UTC (rev 974) +++ knowledge/3rdparty/mary_tts/manifest.xml 2011-10-11 07:00:11 UTC (rev 975) @@ -11,7 +11,7 @@ <review status="unreviewed" notes=""/> <url>http://ros.org/wiki/mary_tts</url> - <depend package="rosjava"/> + <depend package="rosjava_jni"/> <depend package="std_msgs"/> <rosdep name="ant" /> </package> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-10-04 18:24:11
|
Revision: 974 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=974&view=rev Author: moesenle Date: 2011-10-04 18:24:00 +0000 (Tue, 04 Oct 2011) Log Message: ----------- Added tum-ros-pkg.rosinstall for the indexer Added Paths: ----------- rosinstall/tum-ros-pkg.rosinstall Added: rosinstall/tum-ros-pkg.rosinstall =================================================================== --- rosinstall/tum-ros-pkg.rosinstall (rev 0) +++ rosinstall/tum-ros-pkg.rosinstall 2011-10-04 18:24:00 UTC (rev 974) @@ -0,0 +1,36 @@ +- svn: + local-name: tum-ros-pkg + uri: https://tum-ros-pkg.svn.sourceforge.net/svnroot/tum-ros-pkg +- git: + local-name: client_rosjava_jni + uri: http://code.in.tum.de/git/client-rosjava.git +- svn: + local-name: knowrob + uri: http://code.in.tum.de/pubsvn/knowrob/tags/latest +- git: + local-name: ias_perception + uri: http://code.in.tum.de/git/ias-perception.git +- git: + local-name: mapping + uri: http://code.in.tum.de/git/mapping.git +- git: + local-name: cram_physics + uri: git://code.in.tum.de/git/cram-physics.git +- git: + local-name: cram_pl + uri: http://code.in.tum.de/git/cram-pl.git +- git: + local-name: cram_highlevel + uri: http://code.in.tum.de/git/cram-highlevel.git +- git: + local-name: cram_rosie + uri: http://code.in.tum.de/git/cram-rosie.git +- git: + local-name: cram_pr2 + uri: http://code.in.tum.de/git/cram-pr2.git +- git: + local-name: ias_common + uri: http://code.in.tum.de/git/ias-common.git +- git: + local-name: roll + uri: http://code.in.tum.de/git/roll.git This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:02:42
|
Revision: 973 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=973&view=rev Author: moesenle Date: 2011-09-21 14:02:32 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed perception/cop_plugins/cop_sr4_plugins git-commit Removed Paths: ------------- perception/cop_plugins/cop_sr4_plugins/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:02:33
|
Revision: 972 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=972&view=rev Author: moesenle Date: 2011-09-21 14:02:25 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed perception/cop_plugins/cop_ros_plugins git-commit Removed Paths: ------------- perception/cop_plugins/cop_ros_plugins/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:02:23
|
Revision: 971 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=971&view=rev Author: moesenle Date: 2011-09-21 14:02:17 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed perception/cop_plugins/cop_halcon_plugins git-commit Removed Paths: ------------- perception/cop_plugins/cop_halcon_plugins/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:02:19
|
Revision: 970 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=970&view=rev Author: moesenle Date: 2011-09-21 14:02:09 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed perception/cop git-commit Removed Paths: ------------- perception/cop/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:02:12
|
Revision: 969 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=969&view=rev Author: moesenle Date: 2011-09-21 14:02:02 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed perception/cognitive_perception git-commit Removed Paths: ------------- perception/cognitive_perception/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:02:00
|
Revision: 968 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=968&view=rev Author: moesenle Date: 2011-09-21 14:01:55 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed locations/lo git-commit Removed Paths: ------------- locations/lo/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:01:58
|
Revision: 967 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=967&view=rev Author: moesenle Date: 2011-09-21 14:01:47 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed locations/jlo git-commit Removed Paths: ------------- locations/jlo/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:01:46
|
Revision: 966 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=966&view=rev Author: moesenle Date: 2011-09-21 14:01:37 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed 3rdparty/libwww git-commit Removed Paths: ------------- 3rdparty/libwww/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-21 14:01:39
|
Revision: 965 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=965&view=rev Author: moesenle Date: 2011-09-21 14:01:28 +0000 (Wed, 21 Sep 2011) Log Message: ----------- Removed 3rdparty/dxflib git-commit Removed Paths: ------------- 3rdparty/dxflib/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-19 13:22:22
|
Revision: 964 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=964&view=rev Author: moesenle Date: 2011-09-19 13:22:16 +0000 (Mon, 19 Sep 2011) Log Message: ----------- Author: Moritz Tenorth <te...@cs...> Date: Mon Sep 19 15:21:25 2011 +0200 commit 99727025e31d594e72f199110d9be4e20fe9510c Author: Moritz Tenorth <te...@cs...> Date: Wed Aug 17 17:59:12 2011 +0200 argument problem commit 2ad0982caa4e69f2a7b31edeff910c38b5acee3b Author: Moritz Tenorth <te...@cs...> Date: Wed Aug 17 17:50:50 2011 +0200 fixed problem with null-pointer exception in OWL conversion service commit 45828b513a4d9b1e94e78ef78988ba333fc82711 Author: Moritz Tenorth <te...@cs...> Date: Wed Aug 17 09:45:41 2011 +0200 minor bug: show loaded objects immediately commit 15288c970c66e1c39b87ec6c88e06dda4e736479 Author: Moritz Tenorth <te...@cs...> Date: Tue Aug 16 07:34:53 2011 +0200 updated rosjava reference commit 1f6dbe34d06b90051c023d96bf388c48eb15a9c2 Author: Moritz Tenorth <te...@cs...> Date: Tue Aug 16 07:22:42 2011 +0200 forgotten to commit commit ac1b0aec82ded81e890f5f8f5f446e2d5f163d52 Author: Moritz Tenorth <te...@cs...> Date: Thu Jul 7 14:25:29 2011 +0200 public export commit 7d0d3a5f87973920b37dbc515586fc8b69c34dc7 Author: Moritz Tenorth <te...@cs...> Date: Thu Jul 7 14:24:54 2011 +0200 re-named OWLExport commit 692f22a5da97686a02bfed362fc0c45e0c984fd5 Author: Moritz Tenorth <te...@cs...> Date: Thu Jul 7 14:24:26 2011 +0200 API documentation git-commit Modified Paths: -------------- knowledge/mod_semantic_map/launch/mod_semantic_map.launch knowledge/mod_semantic_map/manifest.xml knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapEditorForms.java knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/ROSUtils.java Added Paths: ----------- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/owl/OWLImportExport.java Modified: knowledge/mod_semantic_map/launch/mod_semantic_map.launch =================================================================== --- knowledge/mod_semantic_map/launch/mod_semantic_map.launch 2011-09-19 11:08:44 UTC (rev 963) +++ knowledge/mod_semantic_map/launch/mod_semantic_map.launch 2011-09-19 13:22:16 UTC (rev 964) @@ -1,3 +1,3 @@ <launch> <node name="mod_semantic_map" pkg="mod_semantic_map" type="SemanticMapToOWL" /> -</launch> \ No newline at end of file +</launch> Modified: knowledge/mod_semantic_map/manifest.xml =================================================================== --- knowledge/mod_semantic_map/manifest.xml 2011-09-19 11:08:44 UTC (rev 963) +++ knowledge/mod_semantic_map/manifest.xml 2011-09-19 13:22:16 UTC (rev 964) @@ -11,7 +11,7 @@ <license>BSD</license> <review status="unreviewed" notes=""/> <url>http://ros.org/wiki/mod_semantic_map</url> - <depend package="rosjava" /> + <depend package="rosjava_jni" /> <depend package="geometry_msgs" /> <depend package="ias_knowledge_base" /> <depend package="mod_vis" /> Modified: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapEditorForms.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapEditorForms.java 2011-09-19 11:08:44 UTC (rev 963) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapEditorForms.java 2011-09-19 13:22:16 UTC (rev 964) @@ -126,6 +126,7 @@ if (filename != null) { this.objects = OWLImportExport.readMapObjectFromOWL(filename); } + updateVisualization(); } Modified: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java 2011-09-19 11:08:44 UTC (rev 963) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java 2011-09-19 13:22:16 UTC (rev 964) @@ -96,7 +96,7 @@ } System.out.println("Using map id: " + map_id); - OWLOntology owlmap = export.createOWLMapDescription(map_id, semMapObj2MapObj(req.map.objects)); + OWLOntology owlmap = export.createOWLMapDescription(map_id, semMapObj2MapObj(map_id, req.map.objects)); res.owlmap = OWLFileUtils.saveOntologytoString(owlmap, owlmap.getOWLOntologyManager().getOntologyFormat(owlmap)); } @@ -106,7 +106,7 @@ } - private ArrayList<MapObject> semMapObj2MapObj(ArrayList<SemMapObject> smos) { + private ArrayList<MapObject> semMapObj2MapObj(String map_id, ArrayList<SemMapObject> smos) { HashMap<Integer, MapObject> intIdToID = new HashMap<Integer, MapObject>(); ArrayList<MapObject> mos = new ArrayList<MapObject>(); @@ -130,7 +130,8 @@ } } - intIdToID.get(smo.partOf).physicalParts.add(mo); + if(intIdToID.get(smo.partOf) != null) + intIdToID.get(smo.partOf).physicalParts.add(mo); mos.add(mo); } @@ -143,16 +144,16 @@ public static void main(String[] args) { - if (args.length == 0) { +// if (args.length == 0) { // run service new SemanticMapToOWL(); - } else { - System.out.println("usage: rosrun mod_semantic_map SemanticMapToOWL"); - System.out.println("Commands:"); - System.out.println(" rosrun mod_semantic_map SemanticMapToOWL Runs the service"); - System.out.println(); - } +// } else { +// System.out.println("usage: rosrun mod_semantic_map SemanticMapToOWL"); +// System.out.println("Commands:"); +// System.out.println(" rosrun mod_semantic_map SemanticMapToOWL Runs the service"); +// System.out.println(); +// } } Modified: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java 2011-09-19 11:08:44 UTC (rev 963) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java 2011-09-19 13:22:16 UTC (rev 964) @@ -39,6 +39,7 @@ // req.map.objects = new SemMapObject[4]; + // create cupboard req.map.objects.add(new SemMapObject()); req.map.objects.get(0).id=1; @@ -98,7 +99,7 @@ 0.0f,0.0f,0.0f,1.0f}; ServiceClient<GenerateSemanticMapOWL.Request, GenerateSemanticMapOWL.Response, GenerateSemanticMapOWL> cl = - n.serviceClient("/generate_owl_map", new GenerateSemanticMapOWL()); + n.serviceClient("/knowrob_semantic_map_to_owl/generate_owl_map", new GenerateSemanticMapOWL()); try { GenerateSemanticMapOWL.Response res = cl.call(req); Modified: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/ROSUtils.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/ROSUtils.java 2011-09-19 11:08:44 UTC (rev 963) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/ROSUtils.java 2011-09-19 13:22:16 UTC (rev 964) @@ -7,6 +7,9 @@ * * Utilities related to the ROS system * + * + * + * @deprecated about to be moved into the knowrob_common package * @author Moritz Tenorth, te...@cs... * */ Added: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/owl/OWLImportExport.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/owl/OWLImportExport.java (rev 0) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/utils/owl/OWLImportExport.java 2011-09-19 13:22:16 UTC (rev 964) @@ -0,0 +1,542 @@ +package edu.tum.cs.ias.knowrob.utils.owl; + +import java.io.*; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.Map; +import java.util.Set; + +import javax.vecmath.Matrix4d; + +import org.semanticweb.owlapi.apibinding.OWLManager; +import org.semanticweb.owlapi.io.RDFXMLOntologyFormat; +import org.semanticweb.owlapi.model.*; +import org.semanticweb.owlapi.util.DefaultPrefixManager; +import org.semanticweb.owlapi.reasoner.*; +import org.semanticweb.owlapi.reasoner.structural.*; + +import edu.tum.cs.ias.knowrob.utils.ROSUtils; + + + +/** + * + * Utilities for the import and export of OWL files + * from/to Java data structures + * + * @author Moritz Tenorth, te...@cs... + * @author Lars Kunze, ku...@cs... + * + */ + + +public class OWLImportExport { + + + //////////////////////////////////////////////////////////////////////////////// + // Set IRIs for the ontologies used here + // + + // Base IRI for KnowRob ontology + public final static String KNOWROB = "http://ias.cs.tum.edu/kb/knowrob.owl#"; + + // Base IRI for OWL ontology + public final static String OWL = "http://www.w3.org/2002/07/owl#"; + + // Base IRI for RDFS + public final static String RDFS = "http://www.w3.org/2000/01/rdf-schema#"; + + // Base IRI for semantic map ontology + public final static String IAS_MAP = "http://ias.cs.tum.edu/kb/ias_semantic_map.owl#"; + + // ROS package name for KnowRob + public final static String KNOWROB_PKG = "ias_knowledge_base"; + + // OWL file of the KnowRob ontology (relative to KNOWROB_PKG) + public final static String KNOWROB_OWL = "owl/knowrob.owl"; + + // Prefix manager + public final static DefaultPrefixManager PREFIX_MANAGER = new DefaultPrefixManager(IAS_MAP); + static { + PREFIX_MANAGER.setPrefix("knowrob:", KNOWROB); + PREFIX_MANAGER.setPrefix("ias_map:", IAS_MAP); + PREFIX_MANAGER.setPrefix("owl:", OWL); + PREFIX_MANAGER.setPrefix("rdfs:", RDFS); + } + + // mapping ROS-KnowRob identifiers + protected static final HashMap<String, String> rosToKnowrob = new HashMap<String, String>(); + + OWLDataFactory factory; + OWLOntologyManager manager; + DefaultPrefixManager pm; + + int inst_counter=0; // counter to create unique instance identifiers + + public OWLImportExport() { + //readKnowRobObjectClasses(); + } + + + /** + * Build a complete semantic map, including name spaces, all contained objects, + * and the links between them. + * + * @param map + * @return + */ + public OWLOntology createOWLMapDescription(String map_id, ArrayList<MapObject> map) { + + OWLOntology ontology = null; + HashMap<String, OWLNamedIndividual> idToInst = new HashMap<String, OWLNamedIndividual>(); + + try { + + // Create ontology manager and data factory + manager = OWLManager.createOWLOntologyManager(); + factory = manager.getOWLDataFactory(); + + // Get prefix manager using the base IRI of the JoystickDrive ontology as default namespace + pm = PREFIX_MANAGER; + + // Create empty OWL ontology + ontology = manager.createOntology(IRI.create(map_id)); + manager.setOntologyFormat(ontology, new RDFXMLOntologyFormat()); + + // Import KnowRob ontology + OWLImportsDeclaration oid = factory.getOWLImportsDeclaration(IRI.create(KNOWROB)); + AddImport addImp = new AddImport(ontology,oid); + manager.applyChange(addImp); + + + // create SemanticMap object in the ontology + idToInst.put(map_id, createSemMapInst(ontology)); + + // create time point + OWLNamedIndividual time_inst = createTimePointInst(ros.communication.Time.now(), ontology); + + + // iterate over all objects and create the respective OWL representations + for(MapObject map_obj : map) { + idToInst.put(map_obj.id, createSemMapObjectDescription(map_obj, time_inst, ontology)); + } + + + // link to parent objects (second loop to avoid problems due to wrong ordering) + for(MapObject map_obj : map) { + OWLNamedIndividual obj_inst = idToInst.get(map_obj.id); + + // link high-level objects to the map + if(map_obj.types.contains("Cupboard") || + map_obj.types.contains("Drawer") || + map_obj.types.contains("Oven") || + map_obj.types.contains("Refrigerator") || + map_obj.types.contains("Dishwasher") || + map_obj.types.contains("Table") || + map_obj.types.contains("CounterTop") || + map_obj.types.contains("Sink")) { + + // top-level object, link to map + OWLObjectProperty describedInMap = factory.getOWLObjectProperty("knowrob:describedInMap", pm); + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(describedInMap, obj_inst, idToInst.get(map_id))); + + + // link object components based on their types + } + + for(MapObject p: map_obj.physicalParts) { + + OWLIndividual part = idToInst.get(p.id); + + if(p.types.contains("Door")) { + + // doors are part of parent and may be hinged to it + OWLObjectProperty properPhysicalParts = factory.getOWLObjectProperty("knowrob:properPhysicalParts", pm); + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(properPhysicalParts, obj_inst, part)); + + + // search if map_obj has a rotational joint as part and set the hingedTo property in that case + for(MapObject pi : map_obj.physicalParts) { + + if(pi.types.contains("HingedJoint")) { + OWLObjectProperty hingedTo = factory.getOWLObjectProperty("knowrob:hingedTo", pm); + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(hingedTo, part, obj_inst)); + } + } + + } else { + + // other objects are only part of their parents + OWLObjectProperty properPhysicalParts = factory.getOWLObjectProperty("knowrob:properPhysicalParts", pm); + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(properPhysicalParts, obj_inst, part)); + + } + } + } + + } catch (Exception e) { + ontology = null; + e.printStackTrace(); + } + + return ontology; + } + + + + /** + * Create the OWL description for an object, including the object instance with its dimension, + * a pose instance where the object has been detected, and a SemanticMapPerception instance + * linking object, pose, and detection time + * + * @param map_obj MapObject input data + * @param timestamp OWLIndividual for the time when map_obj has been perceived + * @param ontology Ontology to which the axioms are to be added + */ + public OWLNamedIndividual createSemMapObjectDescription(MapObject map_obj, OWLNamedIndividual timestamp, OWLOntology ontology) { + + // create object instance + OWLNamedIndividual obj_inst = createObjectInst(map_obj, ontology); + + // create pose matrix instance + OWLNamedIndividual pose_inst = createPoseInst(map_obj.pose_matrix, ontology); + + // create perception instance + createPerceptionInst("knowrob:SemanticMapPerception", obj_inst, pose_inst, timestamp, ontology); + + return obj_inst; + } + + + /** + * Create an instance of a knowrob:SemanticEnvironmentMap + * + * @param ontology Ontology to which the axioms are to be added + * @return Created instance of a SemanticEnvironmentMap + */ + public OWLNamedIndividual createSemMapInst(OWLOntology ontology) { + + OWLClass sem_map_class = factory.getOWLClass("knowrob:SemanticEnvironmentMap", pm); + OWLNamedIndividual sem_map_inst = factory.getOWLNamedIndividual( + instForClass("ias_map:SemanticEnvironmentMap"), pm); + manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(sem_map_class, sem_map_inst)); + + return sem_map_inst; + } + + /** + * Generate an instance of the object class indicated by map_obj.type and link it to its parent object + * + * @param map_obj MapObject input data + * @param ontology Ontology to which the axioms are to be added + * @return Created instance of the respective object + */ + public OWLNamedIndividual createObjectInst(MapObject map_obj, OWLOntology ontology) { + + + OWLNamedIndividual obj_inst = factory.getOWLNamedIndividual("knowrob:"+map_obj.id, pm); + + for(String t : map_obj.types) { + OWLClass obj_class = factory.getOWLClass("knowrob:"+t, pm); + manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(obj_class, obj_inst)); + } + + // set object dimensions + OWLDataProperty width = factory.getOWLDataProperty("knowrob:widthOfObject", pm); + OWLDataProperty depth = factory.getOWLDataProperty("knowrob:depthOfObject", pm); + OWLDataProperty height = factory.getOWLDataProperty("knowrob:heightOfObject", pm); + + manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(depth, obj_inst, map_obj.dimensions.x)); + manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(width, obj_inst, map_obj.dimensions.y)); + manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(height, obj_inst, map_obj.dimensions.z)); + + return obj_inst; + } + + /** + * Create an instance of a knowrob:TimePoint for time t + * + * @param t Time to be translated into a TimePoint + * @param ontology Ontology to which the axioms are to be added + * @return Created instance of a TimePoint + */ + public OWLNamedIndividual createTimePointInst(ros.communication.Time stamp, OWLOntology ontology) { + + OWLNamedIndividual time_inst = factory.getOWLNamedIndividual("ias_map:timepoint_"+stamp.secs, pm); + OWLClass time_class = factory.getOWLClass("knowrob:TimePoint", pm); + manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(time_class, time_inst)); + + return time_inst; + } + + + /** + * Create a RotationMatrix3D with all m_ij components set according to the pose of the map_obj + * + * @param map_obj MapObject input data + * @param ontology Ontology to which the axioms are to be added + * @return Created instance of a RotationMatrix3D + */ + public OWLNamedIndividual createPoseInst(Matrix4d pose, OWLOntology ontology) { + + // create pose matrix instance + OWLClass pose_class = factory.getOWLClass("knowrob:RotationMatrix3D", pm); + OWLNamedIndividual pose_inst = factory.getOWLNamedIndividual( + instForClass("knowrob:RotationMatrix3D"), pm); + manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(pose_class, pose_inst)); + + // set pose properties + for(int i=0;i<4;i++) { + for(int j=0;j<4;j++) { + OWLDataProperty prop = factory.getOWLDataProperty("knowrob:m"+i+j, pm); + manager.addAxiom(ontology, factory.getOWLDataPropertyAssertionAxiom(prop, pose_inst, pose.getElement(i,j))); + } + } + + return pose_inst; + } + + + /** + * Create an instance of a SemanticMapPerception linking objects to poses and times + * + * @param type Type of the perception, e.g. "knowrob:SemanticMapPerception" + * @param obj_inst The object that was detected + * @param pose_inst Pose where the object was detected + * @param timestamp Time when the object was detected + * @param ontology Ontology to which the axioms are to be added + * @return Created instance of SemanticMapPerception + */ + public OWLNamedIndividual createPerceptionInst(String type, OWLNamedIndividual obj_inst, OWLNamedIndividual pose_inst, OWLNamedIndividual timestamp, OWLOntology ontology) { + + // create perception instance + OWLClass perc_class = factory.getOWLClass(type, pm); + OWLNamedIndividual perc_inst = factory.getOWLNamedIndividual( + instForClass(type), pm); + manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(perc_class, perc_inst)); + + // link to the object instance and the pose instance + OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); + OWLObjectProperty eventOccursAt = factory.getOWLObjectProperty("knowrob:eventOccursAt", pm); + + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(objectActedOn, perc_inst, obj_inst)); + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(eventOccursAt, perc_inst, pose_inst)); + + // set time stamp + OWLObjectProperty startTime = factory.getOWLObjectProperty("knowrob:startTime", pm); + manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(startTime, perc_inst, timestamp)); + + return perc_inst; + } + + + + /** + * Read a semantic map from an OWL file into a HashMap representation, using the object + * identifier as key and the object pose description as value. + * + * @param filename Name of the OWL file to be imported + * @return HashMap with the object identifiers as key and the MapObject data structures as values + */ + static public HashMap<String, MapObject> readMapObjectFromOWL(String filename) { + + HashMap<String, MapObject> objects = new HashMap<String, MapObject>(); + OWLOntology ont = null; + try { + + OWLOntologyManager manager = OWLManager.createOWLOntologyManager(); + OWLDataFactory factory = manager.getOWLDataFactory(); + DefaultPrefixManager pm = OWLImportExport.PREFIX_MANAGER; + + ont = OWLFileUtils.loadOntologyFromFile(filename); + + if(ont!=null) { + + // iterate over objects, add to objects hashmap + for(OWLNamedIndividual instances : ont.getIndividualsInSignature()) { + + Set<OWLClassExpression> types = instances.getTypes(ont); + OWLClass semanticMapPerception = factory.getOWLClass("knowrob:SemanticMapPerception", pm); + + if(types.contains(semanticMapPerception)) { + + Map<OWLObjectPropertyExpression, Set<OWLIndividual>> perc_props = + instances.getObjectPropertyValues(ont); + + OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); + OWLObjectProperty eventOccursAt = factory.getOWLObjectProperty("knowrob:eventOccursAt", pm); + + Set<OWLIndividual> objs = perc_props.get(objectActedOn); + Set<OWLIndividual> poses = perc_props.get(eventOccursAt); + + + // assuming there is only one object and one pose per perception: + for(OWLIndividual obj : objs) { + for(OWLIndividual pose : poses) { + + // create map object + MapObject cur = new MapObject(); + cur.id = obj.toStringID().split("#")[1]; + + // get types + for(OWLClassExpression c: obj.getTypes(ont)) + cur.types.add(c.asOWLClass().toStringID().split("#")[1]); + + // get dimensions + Map<OWLDataPropertyExpression, Set<OWLLiteral>> data_props = + obj.getDataPropertyValues(ont); + + OWLDataProperty width = factory.getOWLDataProperty("knowrob:widthOfObject", pm); + OWLDataProperty depth = factory.getOWLDataProperty("knowrob:depthOfObject", pm); + OWLDataProperty height = factory.getOWLDataProperty("knowrob:heightOfObject", pm); + + + for(OWLLiteral d : data_props.get(depth)) + cur.dimensions.x = Double.valueOf(d.getLiteral()); + + for(OWLLiteral w : data_props.get(width)) + cur.dimensions.y = Double.valueOf(w.getLiteral()); + + for(OWLLiteral h : data_props.get(height)) + cur.dimensions.z = Double.valueOf(h.getLiteral()); + + + // get pose elements + Map<OWLDataPropertyExpression, Set<OWLLiteral>> matrix_elems = + pose.getDataPropertyValues(ont); + + for(int i=0;i<4;i++) { + for(int j=0;j<4;j++){ + OWLDataProperty m_ij = factory.getOWLDataProperty("knowrob:m"+i+j, pm); + Set<OWLLiteral> elem = matrix_elems.get(m_ij); + + for(OWLLiteral e : elem) { + cur.pose_matrix.setElement(i, j, Double.valueOf(e.getLiteral()) ); + } + + + } + } + objects.put(cur.id, cur); + + } + } + } + } + + for(OWLNamedIndividual instances : ont.getIndividualsInSignature()) { + + Set<OWLClassExpression> types = instances.getTypes(ont); + OWLClass semanticMapPerception = factory.getOWLClass("knowrob:SemanticMapPerception", pm); + + if(types.contains(semanticMapPerception)) { + + Map<OWLObjectPropertyExpression, Set<OWLIndividual>> perc_props = + instances.getObjectPropertyValues(ont); + + OWLObjectProperty objectActedOn = factory.getOWLObjectProperty("knowrob:objectActedOn", pm); + + Set<OWLIndividual> objs = perc_props.get(objectActedOn); + + // link objects to their physical parts + for(OWLIndividual obj : objs) { + + // get proper physical parts + Map<OWLObjectPropertyExpression, Set<OWLIndividual>> obj_props = + obj.getObjectPropertyValues(ont); + + OWLObjectProperty parts = factory.getOWLObjectProperty("knowrob:properPhysicalParts", pm); + + if(obj_props.containsKey(parts)) { + for(OWLIndividual p : obj_props.get(parts)) { + MapObject part = objects.get(p.toStringID().split("#")[1]); + if(part!=null) { + objects.get(obj.toStringID().split("#")[1]).physicalParts.add(part); + } + } + } + } + + } + } + } + + } catch (OWLOntologyCreationException e) { + e.printStackTrace(); + } + return objects; + } + + + + /** + * Initialization of the mapping between object types that are sent via + * the ROS service and concepts of the KnowRob ontology. + */ + protected static void readKnowRobObjectClasses() { + + try { + + // Create ontology manager, data factory, and prefix manager + OWLOntologyManager manager = OWLManager.createOWLOntologyManager(); + OWLDataFactory factory = manager.getOWLDataFactory(); + DefaultPrefixManager pm = PREFIX_MANAGER; + + // Find ros package holding the knowrob ontology + String knowrob_pkg = ROSUtils.rospackFind(KNOWROB_PKG); + String knowrob_owl = knowrob_pkg + "/" + KNOWROB_OWL; + + + // Load the knowrob ontology + OWLOntology ont = manager.loadOntologyFromOntologyDocument(new File(knowrob_owl)); + + // Retrieve only subclasses of SpatialThing-Localized + OWLReasoner reasoner = new StructuralReasoner(ont, new SimpleConfiguration(), BufferingMode.NON_BUFFERING); + OWLClass spatialThing = factory.getOWLClass("knowrob:SpatialThing-Localized", pm); + NodeSet<OWLClass> ns = reasoner.getSubClasses(spatialThing, false); + + java.util.Set<Node<OWLClass>> set = ns.getNodes(); + + // Iterate over all subclasses and put them into the mapping hashmap + for(Node n : set) { + OWLClass c = (OWLClass) n.getRepresentativeElement(); + + String iri = c.toStringID().replaceAll(KNOWROB, "knowrob:"); + String key = c.toStringID().substring(c.toStringID().lastIndexOf('#') + 1).toLowerCase(); + + rosToKnowrob.put(key, iri); + } + // to support backward compatibility (should be removed) + rosToKnowrob.put("hinge", "knowrob:HingedJoint"); + rosToKnowrob.put("knob", "knowrob:ControlKnob"); + rosToKnowrob.put("horizontal_plane", "knowrob:CounterTop"); + + } + catch (Exception e) { + e.printStackTrace(); + } + } + + + /** + * Create a unique instance identifier from a class string + * @param cl Class string + * @return Instance identifier (class string plus index) + */ + protected String instForClass(String cl) { + return cl+(inst_counter++); + } + + + /** + * Debug method: print all object types imported from KnowRob + */ + protected static void printObjectTypes() { + + for(Object o : rosToKnowrob.values()) { + System.out.println(((String)o).replaceAll("knowrob:","")); + } + } + + +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-09-19 11:08:51
|
Revision: 963 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=963&view=rev Author: moesenle Date: 2011-09-19 11:08:44 +0000 (Mon, 19 Sep 2011) Log Message: ----------- Author: Ingo Kresse <kr...@in...> Date: Mon Sep 19 13:06:57 2011 +0200 a simplistic robot simulator for free-space arm movements git-commit Added Paths: ----------- arm_navigation/loopback_controller_manager/ arm_navigation/loopback_controller_manager/CMakeLists.txt arm_navigation/loopback_controller_manager/Makefile arm_navigation/loopback_controller_manager/launch/ arm_navigation/loopback_controller_manager/launch/pr2_cartesian_twist_controllers.launch arm_navigation/loopback_controller_manager/loopback_controller_manager.cpp arm_navigation/loopback_controller_manager/manifest.xml Added: arm_navigation/loopback_controller_manager/CMakeLists.txt =================================================================== --- arm_navigation/loopback_controller_manager/CMakeLists.txt (rev 0) +++ arm_navigation/loopback_controller_manager/CMakeLists.txt 2011-09-19 11:08:44 UTC (rev 963) @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +#set(ROS_BUILD_TYPE RelWithDeb) + +rosbuild_init(rosie_controller_manager) + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) + +#rosbuild_genmsg() +#rosbuild_gensrv() + +#add_definitions(-fPIC) + +rosbuild_add_boost_directories() + +rosbuild_add_executable(loopback_controller_manager loopback_controller_manager.cpp) +rosbuild_link_boost(loopback_controller_manager thread) + + Property changes on: arm_navigation/loopback_controller_manager/CMakeLists.txt ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: arm_navigation/loopback_controller_manager/Makefile =================================================================== --- arm_navigation/loopback_controller_manager/Makefile (rev 0) +++ arm_navigation/loopback_controller_manager/Makefile 2011-09-19 11:08:44 UTC (rev 963) @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file Property changes on: arm_navigation/loopback_controller_manager/Makefile ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: arm_navigation/loopback_controller_manager/launch/pr2_cartesian_twist_controllers.launch =================================================================== --- arm_navigation/loopback_controller_manager/launch/pr2_cartesian_twist_controllers.launch (rev 0) +++ arm_navigation/loopback_controller_manager/launch/pr2_cartesian_twist_controllers.launch 2011-09-19 11:08:44 UTC (rev 963) @@ -0,0 +1,64 @@ +<launch> + + <param name="/use_sim_time" value="false" /> + + <include file="$(find pr2_description)/robots/upload_pr2.launch" /> + + <!-- start loopback controller manager --> + <node name="loopback_controllers" type="loopback_controller_manager" pkg="loopback_controller_manager" output="screen"> + <param name="dt" value="0.003" /> + <param name="damping" value="1.0" /> + <rosparam ns="joints"> + name: ['torso_lift_joint', 'head_tilt_joint', 'r_shoulder_lift_joint', 'r_elbow_flex_joint', 'r_wrist_flex_joint', 'l_shoulder_lift_joint', 'l_elbow_flex_joint', 'l_wrist_flex_joint'] + position: [0.012, 0.20, 0.027, -0.40, -0.66, 0.027, -0.40, -0.66] + + </rosparam> + </node> + + <!-- start joint_states to tf transformer --> + <node pkg="robot_state_chain_publisher" type="state_chain_publisher" name="robot_state_publisher" output="log" /> + + +<!-- <node name="unspawn_arms" + pkg="pr2_controller_manager" type="unspawner" + args="l_arm_controller r_arm_controller" /> +--> + + <rosparam ns="r_cart_twist"> + type: robot_mechanism_controllers/CartesianTwistController + root_name: torso_lift_link + tip_name: r_gripper_tool_frame + fb_trans: + p: 20.0 + i: 0.5 + d: 0.0 + i_clamp: 1.0 + fb_rot: + p: 0.5 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + </rosparam> + + <rosparam ns="l_cart_twist"> + type: robot_mechanism_controllers/CartesianTwistController + root_name: torso_lift_link + tip_name: l_gripper_tool_frame + fb_trans: + p: 20.0 + i: 0.5 + d: 0.0 + i_clamp: 1.0 + fb_rot: + p: 0.5 + i: 0.1 + d: 0.0 + i_clamp: 0.2 + </rosparam> + + + <node name="spawn_cart" + pkg="pr2_controller_manager" type="spawner" + args="l_cart_twist r_cart_twist" /> + +</launch> Added: arm_navigation/loopback_controller_manager/loopback_controller_manager.cpp =================================================================== --- arm_navigation/loopback_controller_manager/loopback_controller_manager.cpp (rev 0) +++ arm_navigation/loopback_controller_manager/loopback_controller_manager.cpp 2011-09-19 11:08:44 UTC (rev 963) @@ -0,0 +1,362 @@ +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include <math.h> +#include <unistd.h> +#include <set> + +#include <boost/thread.hpp> + +#include "pr2_hardware_interface/hardware_interface.h" +#include "pr2_controller_manager/controller_manager.h" +#include "pr2_mechanism_model/robot.h" +#include "tinyxml/tinyxml.h" + +#include <XmlRpcValue.h> +#include <XmlRpcException.h> + +#include <ros/ros.h> + +#include <urdf/model.h> + +class LoopbackControllerManager +{ +public: + LoopbackControllerManager(); + virtual ~LoopbackControllerManager(); + + // Inherited from gazebo::Controller + void init(); + void update(); + void fini(); + + void run(); +private: + + pr2_hardware_interface::HardwareInterface hw_; + pr2_controller_manager::ControllerManager *cm_; + + pr2_mechanism_model::RobotState *state_; + + void readUrdf(); + + ros::NodeHandle* rosnode_; //!< pointer to ros node + ros::Subscriber desired_angles_sub_; + + void ControllerManagerROSThread(); + void jointCallback(const sensor_msgs::JointState::ConstPtr &joints); + boost::thread* ros_spinner_thread_; + boost::mutex lock; + + double dt_; //!< simulation time step + double damping_; //!< default joint damping + double mass_; //!< fake link mass + + void simulateJoints(); +}; + + +using namespace std; +using namespace XmlRpc; + + +LoopbackControllerManager::LoopbackControllerManager() + : hw_(), state_(NULL), dt_(0.0) +{ +} + + +LoopbackControllerManager::~LoopbackControllerManager() +{ + delete cm_; + delete rosnode_; + delete ros_spinner_thread_; +} + +void LoopbackControllerManager::jointCallback(const sensor_msgs::JointState::ConstPtr &joints) +{ + boost::mutex::scoped_lock mutex(lock); + + if(joints->name.size() != joints->position.size()) + { + ROS_ERROR("received invalid desired joints."); + return; + } + + for(unsigned int i=0; i < joints->name.size(); ++i) + { + state_->getJointState(joints->name[i])->position_ = joints->position[i]; + } + +} + +void LoopbackControllerManager::init() +{ + rosnode_ = new ros::NodeHandle("/"); + cm_ = new pr2_controller_manager::ControllerManager(&hw_,*rosnode_); + readUrdf(); // read urdf, setup actuators, then setup mechanism control node + + ros::NodeHandle private_node = ros::NodeHandle("~"); + private_node.param("dt", dt_, 0.01); + private_node.param("damping", damping_, 0.1); + private_node.param("mass", mass_, 0.1); + + desired_angles_sub_ = private_node.subscribe("desired_joints", 1, &LoopbackControllerManager::jointCallback, this); + + state_ = cm_->state_; + + // set all joints to zero + for(unsigned int i = 0; i < state_->joint_states_.size(); i++) + { + pr2_mechanism_model::JointState &s = state_->joint_states_[i]; + s.position_ = 0.0; + s.velocity_ = 0.0; + s.measured_effort_ = 0.0; + s.commanded_effort_ = 0.0; + } + + // set some joints to different starting positions + try + { + XmlRpcValue jointLists, nameList, valueList; + if(private_node.getParam("joints", jointLists)) + { + nameList = jointLists["name"]; + valueList = jointLists["position"]; + for (int index = 0; index < nameList.size(); index++) + { + pr2_mechanism_model::JointState *s = + state_->getJointState((string) nameList[index]); + if(s) + s->position_ = (double) valueList[index]; + } + } + } + catch(XmlRpcException e) + { + // syntax error, print the (pretty useless) error message + ROS_WARN("Error parsing initial joint positions: %s", e.getMessage().c_str()); + } + + + // adjust joint positions to not violate soft limits + for(unsigned int i = 0; i < state_->joint_states_.size(); i++) + { + pr2_mechanism_model::JointState &s = state_->joint_states_[i]; + const boost::shared_ptr<urdf::JointSafety> safety = s.joint_->safety; + + if(safety) + { + double lower = safety->soft_lower_limit; + double upper = safety->soft_upper_limit; + if(lower != 0.0 || upper != 0.0) + { + double margin = (upper - lower) * 0.001; + lower += margin; + upper -= margin; + double pos_old = s.position_; + + s.position_ = (s.position_ < lower) ? lower : s.position_; + s.position_ = (s.position_ > upper) ? upper : s.position_; + + if(s.position_ != pos_old) + ROS_INFO("adjusted joint %s to %f", s.joint_->name.c_str(), s.position_); + } + } + } + + hw_.current_time_ = ros::Time::now(); + + // pr2_etherCAT calls ros::spin(), we'll thread out one spinner here to mimic that + ros_spinner_thread_ = new boost::thread( boost::bind( &LoopbackControllerManager::ControllerManagerROSThread,this ) ); + +} + + +void LoopbackControllerManager::simulateJoints() +{ + // \todo: We should use KDL to do correct dynamics + + for (unsigned int i = 0; i < state_->joint_states_.size(); ++i) + { + pr2_mechanism_model::JointState &s = state_->joint_states_[i]; + const boost::shared_ptr<urdf::JointDynamics> dynamics = state_->joint_states_[i].joint_->dynamics; + + // use damping from urdf (if specified) + double damp = (dynamics) ? dynamics->damping : damping_; + double effort = s.commanded_effort_ - damp*s.velocity_; + + double dv = effort/mass_*dt_; + s.velocity_ += dv; + + double dx = s.velocity_*dt_; + s.position_ += dx; + } +} + + +void LoopbackControllerManager::update() +{ + boost::mutex::scoped_lock mutex(lock); + + // Copies the state from the gazebo joints into the mechanism joints. + for (unsigned int i = 0; i < state_->joint_states_.size(); ++i) + { + state_->joint_states_[i].measured_effort_ = state_->joint_states_[i].commanded_effort_; + } + + // Reverses the transmissions to propagate the joint position into the actuators. + state_->propagateJointPositionToActuatorPosition(); + + //-------------------------------------------------- + // Runs Mechanism Control + //-------------------------------------------------- + + // \todo: necessary? + hw_.current_time_ = ros::Time::now(); + + try + { + cm_->update(); + } + catch (const char* c) + { + if (strcmp(c,"dividebyzero")==0) + ROS_WARN("pid controller reports divide by zero error"); + else + ROS_WARN("unknown const char* exception: %s", c); + } + + //-------------------------------------------------- + // Simulate the effort commands + //-------------------------------------------------- + + // Reverses the transmissions to propagate the actuator commands into the joints. + state_->propagateActuatorEffortToJointEffort(); + + simulateJoints(); +} + +void LoopbackControllerManager::fini() +{ + ROS_DEBUG("calling LoopbackControllerManager::fini"); + + //pr2_hardware_interface::ActuatorMap::const_iterator it; + //for (it = hw_.actuators_.begin(); it != hw_.actuators_.end(); ++it) + // delete it->second; // why is this causing double free corrpution? + cm_->~ControllerManager(); + rosnode_->shutdown(); + + ros_spinner_thread_->join(); +} + +void LoopbackControllerManager::readUrdf() +{ + std::string urdf_string; + rosnode_->getParam("/robot_description", urdf_string); + + // initialize TiXmlDocument doc with a string + TiXmlDocument doc; + if (!doc.Parse(urdf_string.c_str())) + { + ROS_ERROR("Failed to load robot description"); + abort(); + } + + struct GetActuators : public TiXmlVisitor + { + std::set<std::string> actuators; + virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *) + { + if (elt.Attribute("name") && + ( elt.ValueStr() == std::string("actuator") + || elt.ValueStr() == std::string("rightActuator") + || elt.ValueStr() == std::string("leftActuator") )) + actuators.insert(elt.Attribute("name")); + return true; + } + } get_actuators; + doc.RootElement()->Accept(&get_actuators); + + // Places the found actuators into the hardware interface. + std::set<std::string>::iterator it; + for (it = get_actuators.actuators.begin(); it != get_actuators.actuators.end(); ++it) + { + // ROS_INFO_STREAM("adding actuator " << *it); + pr2_hardware_interface::Actuator* pr2_actuator = new pr2_hardware_interface::Actuator(*it); + pr2_actuator->state_.is_enabled_ = true; + hw_.addActuator(pr2_actuator); + } + + // Setup mechanism control node + cm_->initXml(doc.RootElement()); + + for (unsigned int i = 0; i < cm_->state_->joint_states_.size(); ++i) + cm_->state_->joint_states_[i].calibrated_ = true; +} + + +void LoopbackControllerManager::ControllerManagerROSThread() +{ + ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id()); + + ros::Rate rate(0.2/dt_); + + while (rosnode_->ok()) + { + rate.sleep(); + ros::spinOnce(); + } +} + + +void LoopbackControllerManager::run() +{ + + ros::Rate rate(1.0/dt_); + while(rosnode_->ok()) + { + update(); + rate.sleep(); + } +} + + +int main(int argc, char *argv[]) +{ + ros::init(argc,argv,"loopback_controllers"); + + // bring up mechanism controllers + LoopbackControllerManager m; + + m.init(); + m.run(); + m.fini(); +} Property changes on: arm_navigation/loopback_controller_manager/loopback_controller_manager.cpp ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: arm_navigation/loopback_controller_manager/manifest.xml =================================================================== --- arm_navigation/loopback_controller_manager/manifest.xml (rev 0) +++ arm_navigation/loopback_controller_manager/manifest.xml 2011-09-19 11:08:44 UTC (rev 963) @@ -0,0 +1,17 @@ +<package> + <description brief="loop back simulation for pr2 realtime controllers"> + controller manager that does a very simple simulation of joints: + It just "loops back" effort to joint velocity / position. + </description> + <author>Ingo Kresse</author> + <license>BSD</license> + <review status="unreviewed"/> + <depend package="roscpp"/> + <depend package="robot_mechanism_controllers" /> + <depend package="pr2_hardware_interface" /> + <depend package="pr2_controller_manager" /> + <depend package="pr2_mechanism_model" /> + <depend package="tinyxml" /> + <depend package="urdf" /> + +</package> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-08-30 09:20:36
|
Revision: 962 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=962&view=rev Author: moesenle Date: 2011-08-30 09:20:30 +0000 (Tue, 30 Aug 2011) Log Message: ----------- Removed utils/rosjava_deps git-commit Removed Paths: ------------- utils/rosjava_deps/ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-08-26 08:33:44
|
Revision: 960 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=960&view=rev Author: moesenle Date: 2011-08-26 08:33:38 +0000 (Fri, 26 Aug 2011) Log Message: ----------- Author: Ingo Kresse <kr...@in...> Date: Fri Aug 26 10:29:14 2011 +0200 Another EtherCAT Master library This pacakge checks out the IgH EtherCAT Master library and compiles the user space library only. Compiling the kernel modules is a rather manual thing... git-commit Added Paths: ----------- 3rdparty/igh_eml/ 3rdparty/igh_eml/.gitignore 3rdparty/igh_eml/Makefile 3rdparty/igh_eml/manifest.xml Added: 3rdparty/igh_eml/.gitignore =================================================================== --- 3rdparty/igh_eml/.gitignore (rev 0) +++ 3rdparty/igh_eml/.gitignore 2011-08-26 08:33:38 UTC (rev 960) @@ -0,0 +1,5 @@ +downloaded +include/ +installed +lib/ +rospack_nosubdirs Added: 3rdparty/igh_eml/Makefile =================================================================== --- 3rdparty/igh_eml/Makefile (rev 0) +++ 3rdparty/igh_eml/Makefile 2011-08-26 08:33:38 UTC (rev 960) @@ -0,0 +1,37 @@ + +SOURCE_DIR=build/igh_eml + +all: installed + +downloaded: Makefile + rm -rf build + mkdir -p build + hg clone http://etherlabmaster.hg.sourceforge.net:8000/hgroot/etherlabmaster/etherlabmaster $(SOURCE_DIR) + #hg clone $(HOME)/work/base/ethercat-default $(SOURCE_DIR) + cd $(SOURCE_DIR); hg checkout -r e9f722488fcd + touch $(SOURCE_DIR)/config.h + touch rospack_nosubdirs + touch downloaded + +installed: downloaded + mkdir -p include lib + make libethercat + ln -sf ../$(SOURCE_DIR)/include include/igh_eml + + touch installed + +clean: + rm -rf build + rm -f downloaded rospack_nosubdirs installed + + +# direct compile rules. We only need the library, so we can get rid of the kernel-header dependency + +LIB_OBJ=$(patsubst %.c,%.o,$(wildcard $(SOURCE_DIR)/lib/*.c)) + +libethercat: $(LIB_OBJ) + echo $(LIB_OBJ) + gcc -shared -Wl,-soname -Wl,libethercat.so -o lib/libethercat.so $(LIB_OBJ) + +.c.o: + gcc -I $(SOURCE_DIR) -I $(SOURCE_DIR)/lib -fPIC -o $@ -c $< Property changes on: 3rdparty/igh_eml/Makefile ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: 3rdparty/igh_eml/manifest.xml =================================================================== --- 3rdparty/igh_eml/manifest.xml (rev 0) +++ 3rdparty/igh_eml/manifest.xml 2011-08-26 08:33:38 UTC (rev 960) @@ -0,0 +1,13 @@ +<package> + <description brief="The IgH EtherCAT master library"> +This package compiles the user space library only, so no need to worry about +kernel headers. + </description> + <author>Ingo Kresse</author> + <license>GPL,LGPL</license> + <review status="unreviewed" notes=""/> + + <export> + <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lethercat"/> + </export> +</package> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-07-21 09:18:32
|
Revision: 957 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=957&view=rev Author: moesenle Date: 2011-07-21 09:18:20 +0000 (Thu, 21 Jul 2011) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Thu Jul 21 02:15:09 2011 -0700 commit 9c0e5abe9403c583142462fb41f5cc9b97a7cc15 Author: Lorenz Moesenlechner <moe...@in...> Date: Thu Sep 30 19:03:20 2010 +0200 Added tf_relay package git-commit Added Paths: ----------- utils/tf_relay/ utils/tf_relay/CMakeLists.txt utils/tf_relay/Makefile utils/tf_relay/mainpage.dox utils/tf_relay/manifest.xml utils/tf_relay/scripts/ utils/tf_relay/scripts/tf_relay.py Added: utils/tf_relay/CMakeLists.txt =================================================================== --- utils/tf_relay/CMakeLists.txt (rev 0) +++ utils/tf_relay/CMakeLists.txt 2011-07-21 09:18:20 UTC (rev 957) @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) Property changes on: utils/tf_relay/CMakeLists.txt ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: utils/tf_relay/Makefile =================================================================== --- utils/tf_relay/Makefile (rev 0) +++ utils/tf_relay/Makefile 2011-07-21 09:18:20 UTC (rev 957) @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file Property changes on: utils/tf_relay/Makefile ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: utils/tf_relay/mainpage.dox =================================================================== --- utils/tf_relay/mainpage.dox (rev 0) +++ utils/tf_relay/mainpage.dox 2011-07-21 09:18:20 UTC (rev 957) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b tf_relay is ... + +<!-- +Provide an overview of your package. +--> + + +\section codeapi Code API + +<!-- +Provide links to specific auto-generated API documentation within your +package that is of particular interest to a reader. Doxygen will +document pretty much every part of your code, so do your best here to +point the reader to the actual API. + +If your codebase is fairly large or has different sets of APIs, you +should use the doxygen 'group' tag to keep these APIs together. For +example, the roscpp documentation has 'libros' group. +--> + + +*/ Added: utils/tf_relay/manifest.xml =================================================================== --- utils/tf_relay/manifest.xml (rev 0) +++ utils/tf_relay/manifest.xml 2011-07-21 09:18:20 UTC (rev 957) @@ -0,0 +1,17 @@ +<package> + <description brief="tf_relay"> + + tf_relay + + </description> + <author>Lorenz Moesenlechner</author> + <license>BSD</license> + <review status="unreviewed" notes=""/> + <url>http://ros.org/wiki/tf_relay</url> + <depend package="tf"/> + <depend package="geometry_msgs"/> + <depend package="rospy"/> + +</package> + + Added: utils/tf_relay/scripts/tf_relay.py =================================================================== --- utils/tf_relay/scripts/tf_relay.py (rev 0) +++ utils/tf_relay/scripts/tf_relay.py 2011-07-21 09:18:20 UTC (rev 957) @@ -0,0 +1,44 @@ +#!/usr/bin/env python + +import roslib; roslib.load_manifest('tf_relay') + +import rospy +import tf.msg as tf +import geometry_msgs.msg as geometry_msgs + +class TFRelay(object): + def __init__(self): + self.tf_prefix = rospy.get_param('~tf_prefix') + self.fixed_frame = rospy.get_param('~fixed_frame', '/map') + if self.fixed_frame[0] != '/': + self.fixed_frame = '/' + self.fixed_frame + self.sub = rospy.Subscriber('tf_in', tf.tfMessage, self.callback) + self.pub = rospy.Publisher('tf_out', tf.tfMessage) + + def is_fixed_frame(self, frame_id): + if frame_id[0] == '/' and frame_id == self.fixed_frame: + return True + elif frame_id == self.fixed_frame[1:]: + return True + else: + return False + + def prepend_prefix(self, frame_id): + if frame_id[0] == '/': + return self.tf_prefix + frame_id + else: + return self.tf_prefix + '/' + frame_id + + def callback(self, msg): + for trans in msg.transforms: + if not self.is_fixed_frame(trans.header.frame_id): + trans.header.frame_id = self.prepend_prefix(trans.header.frame_id) + trans.child_frame_id = self.prepend_prefix(trans.child_frame_id) + + self.pub.publish(msg) + +if __name__ == '__main__': + rospy.init_node('tf_relay') + relay = TFRelay() + rospy.spin() + Property changes on: utils/tf_relay/scripts/tf_relay.py ___________________________________________________________________ Added: svn:executable + * This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-07-21 09:18:19
|
Revision: 956 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=956&view=rev Author: moesenle Date: 2011-07-21 09:18:08 +0000 (Thu, 21 Jul 2011) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Thu Jul 21 02:15:09 2011 -0700 commit c8fe2d3a97e7e5fb6c9925db8ef1bfb33c4f4f19 Author: Ingo Kresse <kr...@in...> Date: Sun Jan 23 14:57:03 2011 +0100 reorganizing ias_robot_defs commit 869f07808530698a937d6620a44920a2fc337861 Author: Lorenz Moesenlechner <moe...@in...> Date: Tue Oct 12 21:09:58 2010 +0200 Load icub's model on the parameter server commit d225f0cc33fd95238f8852ad77b66a378ea3f4c1 Author: Lorenz Moesenlechner <moe...@in...> Date: Tue Oct 12 21:07:49 2010 +0200 Added static transform from map to base_link for icub commit 579f9c62883afacd7972d19e16ac28d08c410f6b Author: Lorenz Moesenlechner <moe...@in...> Date: Tue Oct 12 17:50:16 2010 +0200 Launch files for icub visualization together with the other robots commit 191fcf07efc5414600c8af0324fa678a751f54d3 Author: Lorenz Moesenlechner <moe...@in...> Date: Thu Oct 7 11:17:59 2010 +0200 Fixed tf relay topic for pr2 commit fc1065c511fc89f340bbdc0049c5e31534860aa4 Author: Lorenz Moesenlechner <moe...@in...> Date: Thu Sep 30 19:17:44 2010 +0200 Added launch files for visualizing rosie and pr2 in the same rviz git-commit Added Paths: ----------- utils/multi_robot_visualization/ utils/multi_robot_visualization/CMakeLists.txt utils/multi_robot_visualization/Makefile utils/multi_robot_visualization/kitchen.vcg utils/multi_robot_visualization/launch/ utils/multi_robot_visualization/launch/icub.launch utils/multi_robot_visualization/launch/pr2.launch utils/multi_robot_visualization/launch/rosie.launch utils/multi_robot_visualization/launch/visualization.launch utils/multi_robot_visualization/mainpage.dox utils/multi_robot_visualization/manifest.xml Added: utils/multi_robot_visualization/CMakeLists.txt =================================================================== --- utils/multi_robot_visualization/CMakeLists.txt (rev 0) +++ utils/multi_robot_visualization/CMakeLists.txt 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) Property changes on: utils/multi_robot_visualization/CMakeLists.txt ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: utils/multi_robot_visualization/Makefile =================================================================== --- utils/multi_robot_visualization/Makefile (rev 0) +++ utils/multi_robot_visualization/Makefile 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file Property changes on: utils/multi_robot_visualization/Makefile ___________________________________________________________________ Added: svn:mime-type + text/plain Added: svn:eol-style + native Added: utils/multi_robot_visualization/kitchen.vcg =================================================================== --- utils/multi_robot_visualization/kitchen.vcg (rev 0) +++ utils/multi_robot_visualization/kitchen.vcg 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,488 @@ +Background\ ColorR=0 +Background\ ColorG=0 +Background\ ColorB=0 +Fixed\ Frame=/map +Target\ Frame=<Fixed Frame> +Grid.Alpha=0.5 +Grid.Cell\ Size=1 +Grid.ColorR=0.5 +Grid.ColorG=0.5 +Grid.ColorB=0.5 +Grid.Enabled=1 +Grid.Line\ Style=0 +Grid.Line\ Width=0.03 +Grid.Normal\ Cell\ Count=0 +Grid.OffsetX=0 +Grid.OffsetY=0 +Grid.OffsetZ=0 +Grid.Plane=0 +Grid.Plane\ Cell\ Count=10 +Grid.Reference\ Frame=<Fixed Frame> +Kitchen\ Model.Alpha=1 +Kitchen\ Model.Collision\ Enabled=0 +Kitchen\ Model.Enabled=1 +Kitchen\ Model.Robot\ Description=/ias_kitchen/kitchen_description +Kitchen\ Model.TF\ Prefix=/ias_kitchen +Kitchen\ Model.Update\ Interval=0 +Kitchen\ Model.Visual\ Enabled=1 +Map.Alpha=0.7 +Map.Draw\ Behind=1 +Map.Enabled=1 +Map.Topic=/map +Robot\ Model.Alpha=1 +Robot\ Model.Collision\ Enabled=0 +Robot\ Model.Enabled=1 +Robot\ Model.Robot\ Description=rosie_description +Robot\ Model.TF\ Prefix=rosie +Robot\ Model.Update\ Interval=0 +Robot\ Model.Visual\ Enabled=1 +Robot\ Model2.Alpha=1 +Robot\ Model2.Collision\ Enabled=0 +Robot\ Model2.Enabled=1 +Robot\ Model2.Robot\ Description=pr2_description +Robot\ Model2.TF\ Prefix=pr2 +Robot\ Model2.Update\ Interval=0 +Robot\ Model2.Visual\ Enabled=1 +Robot\:\ Kitchen\ Model\ Link\ counter_fridge_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_fridge_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_island_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_island_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_oven_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_oven_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_island_back_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_island_back_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_island_left_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_island_left_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_island_right_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_island_right_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_sink_left_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_sink_left_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_sink_right_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_side_sink_right_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ counter_sink_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ counter_sink_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_fridge_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_fridge_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_fridge_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_fridge_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col1_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col1_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col1_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col1_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col1_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col1_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col2_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col2_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col2_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col2_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col2_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col2_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col3_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col3_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col3_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col3_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col3_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_island_col3_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_left_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_left_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_oven_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_oven_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_right_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_oven_right_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col1_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col1_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col1_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col1_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col1_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col1_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col2_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col2_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col3_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col3_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col3_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ drawer_sink_col3_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ fridge_block_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ fridge_block_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_fridge_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_fridge_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_fridge_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_fridge_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col1_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col1_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col1_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col1_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col1_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col1_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col2_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col2_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col2_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col2_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col2_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col2_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col3_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col3_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col3_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col3_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col3_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_island_col3_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_left_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_left_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_oven_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_oven_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_right_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_oven_right_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col1_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col1_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col1_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col1_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col1_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col1_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col2_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col2_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col3_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col3_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col3_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ handle_drawer_sink_col3_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ island_block_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ island_block_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ oven_block_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ oven_block_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ shelf_bottom_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ shelf_bottom_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ shelf_center_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ shelf_center_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ shelf_top_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ shelf_top_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ sink_block_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ sink_block_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ sink_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ sink_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_fridge_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_fridge_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_island_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_island_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_oven_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_oven_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_sink_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ skirting_sink_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ stove_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ stove_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ wall_main_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ wall_main_linkShow\ Trail=0 +Robot\:\ Kitchen\ Model\ Link\ wall_stickout_linkShow\ Axes=0 +Robot\:\ Kitchen\ Model\ Link\ wall_stickout_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ LeftEyeCalcShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ LeftEyeCalcShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ LeftEyeCalc_sensorShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ LeftEyeCalc_sensorShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ RightEyeCalcShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ RightEyeCalcShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ RightEyeCalc_sensorShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ RightEyeCalc_sensorShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ base_left_back_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ base_left_back_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ base_left_front_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ base_left_front_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ base_right_back_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ base_right_back_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ base_right_front_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ base_right_front_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ calib_left_arm_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ calib_left_arm_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ calib_right_arm_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ calib_right_arm_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ laser_front_leftShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ laser_front_leftShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ laser_rear_rightShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ laser_rear_rightShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_1_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_1_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_2_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_2_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_3_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_3_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_4_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_4_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_5_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_5_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_6_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_6_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_arm_7_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_arm_7_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_fore_finger_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_middle_finger_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_palmShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_palmShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_ring_finger_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_baseShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_baseShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ left_hand_thumb_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ measured_ptu_baseShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ measured_ptu_baseShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ ptu_tilt_centerShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ ptu_tilt_centerShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_1_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_1_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_2_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_2_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_3_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_3_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_4_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_4_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_5_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_5_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_6_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_6_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_arm_7_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_arm_7_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_fore_finger_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_middle_finger_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_palmShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_palmShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_ring_finger_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_baseShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_baseShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_base_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_base_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_distal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_distal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_middle_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_middle_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_proximal_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ right_hand_thumb_proximal_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilt_baseShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilt_baseShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilt_endShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilt_endShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilting_laserShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilting_laserShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilting_laser_sensorShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ shoulder_tilting_laser_sensorShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ sr4Show\ Axes=0 +Robot\:\ Robot\ Model\ Link\ sr4Show\ Trail=0 +Robot\:\ Robot\ Model\ Link\ torso_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ torso_linkShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ wide_stereo_gazebo_l_stereo_camera_frameShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ wide_stereo_gazebo_l_stereo_camera_frameShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ wide_stereo_gazebo_r_stereo_camera_frameShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ wide_stereo_gazebo_r_stereo_camera_frameShow\ Trail=0 +Robot\:\ Robot\ Model\ Link\ wide_stereo_linkShow\ Axes=0 +Robot\:\ Robot\ Model\ Link\ wide_stereo_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ base_footprintShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ base_footprintShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ base_laser_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ base_laser_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ base_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ base_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ bl_caster_l_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ bl_caster_l_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ bl_caster_r_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ bl_caster_r_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ bl_caster_rotation_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ bl_caster_rotation_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ br_caster_l_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ br_caster_l_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ br_caster_r_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ br_caster_r_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ br_caster_rotation_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ br_caster_rotation_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ double_stereo_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ double_stereo_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ fl_caster_l_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ fl_caster_l_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ fl_caster_r_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ fl_caster_r_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ fl_caster_rotation_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ fl_caster_rotation_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ fr_caster_l_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ fr_caster_l_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ fr_caster_r_wheel_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ fr_caster_r_wheel_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ fr_caster_rotation_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ fr_caster_rotation_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ head_pan_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ head_pan_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ head_plate_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ head_plate_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ head_tilt_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ head_tilt_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ high_def_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ high_def_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ high_def_optical_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ high_def_optical_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ imu_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ imu_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_elbow_flex_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_elbow_flex_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_forearm_cam_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_forearm_cam_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_forearm_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_forearm_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_forearm_roll_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_forearm_roll_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_l_finger_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_l_finger_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_l_finger_tip_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_l_finger_tip_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_motor_accelerometer_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_motor_accelerometer_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_palm_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_palm_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_r_finger_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_r_finger_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_r_finger_tip_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_gripper_r_finger_tip_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_shoulder_lift_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_shoulder_lift_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_shoulder_pan_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_shoulder_pan_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_upper_arm_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_upper_arm_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_upper_arm_roll_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_upper_arm_roll_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_wrist_flex_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_wrist_flex_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ l_wrist_roll_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ l_wrist_roll_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ laser_tilt_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ laser_tilt_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ laser_tilt_mount_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ laser_tilt_mount_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ narrow_stereo_gazebo_l_stereo_camera_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ narrow_stereo_gazebo_l_stereo_camera_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ narrow_stereo_gazebo_r_stereo_camera_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ narrow_stereo_gazebo_r_stereo_camera_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ narrow_stereo_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ narrow_stereo_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ projector_wg6802418_child_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ projector_wg6802418_child_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ projector_wg6802418_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ projector_wg6802418_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_elbow_flex_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_elbow_flex_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_forearm_cam_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_forearm_cam_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_forearm_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_forearm_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_forearm_roll_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_forearm_roll_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_l_finger_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_l_finger_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_l_finger_tip_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_l_finger_tip_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_motor_accelerometer_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_motor_accelerometer_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_palm_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_palm_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_r_finger_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_r_finger_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_r_finger_tip_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_gripper_r_finger_tip_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_shoulder_lift_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_shoulder_lift_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_shoulder_pan_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_shoulder_pan_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_upper_arm_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_upper_arm_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_upper_arm_roll_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_upper_arm_roll_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_wrist_flex_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_wrist_flex_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ r_wrist_roll_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ r_wrist_roll_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ sensor_mount_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ sensor_mount_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ torso_lift_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ torso_lift_linkShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ wide_stereo_gazebo_l_stereo_camera_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ wide_stereo_gazebo_l_stereo_camera_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ wide_stereo_gazebo_r_stereo_camera_frameShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ wide_stereo_gazebo_r_stereo_camera_frameShow\ Trail=0 +Robot\:\ Robot\ Model2\ Link\ wide_stereo_linkShow\ Axes=0 +Robot\:\ Robot\ Model2\ Link\ wide_stereo_linkShow\ Trail=0 +Tool\ 2D\ Nav\ GoalTopic=/map_annotation_gui/pose +Tool\ 2D\ Pose\ EstimateTopic=initialpose +Camera\ Type=rviz::OrbitViewController +Camera\ Config=0.821001 2.52159 8.28953 -1.70652 0.463651 2.2165 +Property\ Grid\ State=selection=;expanded=.Global Options;scrollpos=0,0;splitterpos=193,403;ispageselected=1 +[Display0] +Name=Robot Model +Package=rviz +ClassName=rviz::RobotModelDisplay +[Display1] +Name=Grid +Package=rviz +ClassName=rviz::GridDisplay +[Display2] +Name=Map +Package=rviz +ClassName=rviz::MapDisplay +[Display3] +Name=Kitchen Model +Package=rviz +ClassName=rviz::RobotModelDisplay +[Display4] +Name=Robot Model2 +Package=rviz +ClassName=rviz::RobotModelDisplay Added: utils/multi_robot_visualization/launch/icub.launch =================================================================== --- utils/multi_robot_visualization/launch/icub.launch (rev 0) +++ utils/multi_robot_visualization/launch/icub.launch 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,8 @@ +<launch> + <node name="icub_map_transform" + pkg="tf" type="static_transform_publisher" + args="-4.73 2.2 1.0 0 0 -0.682 0.731 /map /base_link 250"/> + <node name="icub_relay_tf" + pkg="foreign_relay" type="foreign_relay" + args="adv $(env FOREIGN_MASTER_URI) /tf_icub /tf"/> +</launch> Added: utils/multi_robot_visualization/launch/pr2.launch =================================================================== --- utils/multi_robot_visualization/launch/pr2.launch (rev 0) +++ utils/multi_robot_visualization/launch/pr2.launch 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,5 @@ +<launch> + <node name="pr2_relay_tf" + pkg="foreign_relay" type="foreign_relay" + args="adv $(env FOREIGN_MASTER_URI) /tf_pr2 /tf"/> +</launch> Added: utils/multi_robot_visualization/launch/rosie.launch =================================================================== --- utils/multi_robot_visualization/launch/rosie.launch (rev 0) +++ utils/multi_robot_visualization/launch/rosie.launch 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,8 @@ +<launch> + <node name="rosie_relay_tf" + pkg="foreign_relay" type="foreign_relay" + args="adv $(env FOREIGN_MASTER_URI) /tf_rosie /tf"/> + <node name="rosie_relay_shoulder_cloud" + pkg="foreign_relay" type="foreign_relay" + args="adv $(env FOREIGN_MASTER_URI) /rosie/shoulder_cloud /shoulder_cloud"/> +</launch> Added: utils/multi_robot_visualization/launch/visualization.launch =================================================================== --- utils/multi_robot_visualization/launch/visualization.launch (rev 0) +++ utils/multi_robot_visualization/launch/visualization.launch 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,31 @@ +<launch> + <param name="rosie_description" command="$(find xacro)/xacro.py '$(find rosie_description)/robots/rosie.urdf.xacro'"/> + <param name="pr2_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'" /> + <param name="icub_description" command="cat $(find icub_robot_defs)/robots/icub_basic.urdf.xml"/> + + <node name="rosie_tf" pkg="tf_relay" type="tf_relay.py"> + <remap from="tf_in" to="/tf_rosie"/> + <remap from="tf_out" to="/tf"/> + <param name="fixed_frame" value="/map"/> + <param name="tf_prefix" value="/rosie"/> + </node> + + <node name="pr2_tf" pkg="tf_relay" type="tf_relay.py"> + <remap from="tf_in" to="/tf_pr2"/> + <remap from="tf_out" to="/tf"/> + <param name="fixed_frame" value="/map"/> + <param name="tf_prefix" value="/pr2"/> + </node> + + <node name="icub_tf" pkg="tf_relay" type="tf_relay.py"> + <remap from="tf_in" to="/tf_icub"/> + <remap from="tf_out" to="/tf"/> + <param name="fixed_frame" value="/map"/> + <param name="tf_prefix" value="/icub"/> + </node> + + <node name="map_server" pkg="map_server" type="map_server" args="$(find ias_maps)/ias_karlstrasse_20100830.yaml" /> + + <include file="$(find ias_kitchen_defs)/launch/ias_kitchen_model.launch"/> + +</launch> Added: utils/multi_robot_visualization/mainpage.dox =================================================================== --- utils/multi_robot_visualization/mainpage.dox (rev 0) +++ utils/multi_robot_visualization/mainpage.dox 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,26 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b multi_robot_visualization is ... + +<!-- +Provide an overview of your package. +--> + + +\section codeapi Code API + +<!-- +Provide links to specific auto-generated API documentation within your +package that is of particular interest to a reader. Doxygen will +document pretty much every part of your code, so do your best here to +point the reader to the actual API. + +If your codebase is fairly large or has different sets of APIs, you +should use the doxygen 'group' tag to keep these APIs together. For +example, the roscpp documentation has 'libros' group. +--> + + +*/ Added: utils/multi_robot_visualization/manifest.xml =================================================================== --- utils/multi_robot_visualization/manifest.xml (rev 0) +++ utils/multi_robot_visualization/manifest.xml 2011-07-21 09:18:08 UTC (rev 956) @@ -0,0 +1,20 @@ +<package> + <description brief="multi_robot_visualization"> + + multi_robot_visualization + + </description> + <author>Lorenz Moesenlechner</author> + <license>BSD</license> + <review status="unreviewed" notes=""/> + <url>http://ros.org/wiki/multi_robot_visualization</url> + <depend package="tf_relay"/> + <depend package="foreign_relay"/> + <depend package="rosie_description"/> + <depend package="pr2_description"/> + <depend package="map_server"/> + <depend package="ias_maps"/> + +</package> + + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-07-14 01:47:36
|
Revision: 955 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=955&view=rev Author: moesenle Date: 2011-07-14 01:47:24 +0000 (Thu, 14 Jul 2011) Log Message: ----------- Author: Dejan Pangercic <dej...@gm...> Date: Wed Jul 13 18:43:17 2011 -0700 commit df26f60468fd99f082d4da4e1f0d44c3da00fce6 Author: Thomas Ruehr <ru...@in...> Date: Tue Jun 7 08:54:53 2011 +0200 added pose query returning tf type commit 7db935abcf09d76b8004eaee728ab23670ccf792 Author: Ulrich Klank <kl...@in...> Date: Mon Apr 4 17:09:44 2011 +0200 smaller changes commit c9cd0487ba267285984d1d6c5deb96dfb3cfca88 Author: Ulrich Klank <kl...@in...> Date: Fri Mar 25 16:19:52 2011 +0100 Paper related changes commit e28b2213817c2a631f614ae1ac94dd3dfa14afb1 Author: Ulrich Klank <kl...@in...> Date: Tue Mar 22 11:43:19 2011 +0100 Experiemnts on rosie commit 01710593fd4f4b1638645afcff9affda72df9ae8 Author: Ulrich Klank <kl...@in...> Date: Tue Mar 15 16:24:09 2011 +0100 Client logs more data commit 2690963cc441c49bb34657f71f3c553c1153d5cd Author: Ulrich Klank <kl...@in...> Date: Mon Mar 14 16:23:58 2011 +0100 cop client improvements and minro fixes commit fe1baf89bd871762902581c4cacf5f6f71e98046 Author: Ulrich Klank <kl...@in...> Date: Wed Mar 9 18:35:53 2011 +0100 Return to home in test_cop commit 2886fe45c52ba99c281e789a5edf9cb943423c58 Author: Ulrich Klank <kl...@in...> Date: Wed Mar 9 13:27:05 2011 +0100 Fixed stupid bug in projection of 3D image commit bfe43d8105433fd5ff13afa0463fa6eed8e93a95 Author: Ulrich Klank <kl...@in...> Date: Mon Mar 7 19:32:15 2011 +0100 Fixing errors, more test stuff commit ceef51729f059090284102b2bd18c291a8e7a874 Author: Ulrich Klank <kl...@in...> Date: Mon Mar 7 18:56:39 2011 +0100 Adding new testing resources commit 9f3efdff6727358ce907f1a4e38d050274cd3509 Author: Ulrich Klank <kl...@in...> Date: Mon Mar 7 16:11:56 2011 +0100 new test scripts commit d4f5f48f69c3ef2f1720c33b1285dab0f3d89616 Author: Thomas Ruehr <ru...@in...> Date: Thu Oct 7 13:45:13 2010 +0200 missing header commit 7dc12b07285bb8f638b93ad1041e980147221cc3 Author: Thomas Ruehr <ru...@in...> Date: Mon Oct 4 12:08:38 2010 +0200 plate detection adapted for ias_drawer_executive commit bc5a994a27397eae2868392e4aa5726b0e9c579d Author: Ulrich Klank <kl...@in...> Date: Mon Sep 27 18:31:53 2010 +0200 Made cop_lient_cpp work on pr2 commit b83c3b6169c99d3d3c0b138f0bd25421af16a980 Author: Ulrich Friedrich Klank <kl...@in...> Date: Mon Sep 27 18:18:00 2010 +0200 Added a preliminary cpp client for using cop git-commit Added Paths: ----------- perception/cop_client_cpp/ perception/cop_client_cpp/CMakeLists.txt perception/cop_client_cpp/Makefile perception/cop_client_cpp/include/ perception/cop_client_cpp/include/cop_client_cpp/ perception/cop_client_cpp/include/cop_client_cpp/cop_client.h perception/cop_client_cpp/manifest.xml perception/cop_client_cpp/src/ perception/cop_client_cpp/src/cop_client.cpp perception/cop_client_cpp/src/refinements.cpp perception/cop_client_cpp/src/test_cop.cpp Added: perception/cop_client_cpp/CMakeLists.txt =================================================================== --- perception/cop_client_cpp/CMakeLists.txt (rev 0) +++ perception/cop_client_cpp/CMakeLists.txt 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1,19 @@ +PROJECT(testclient) + +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) +rosbuild_init() + +include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) + + +rosbuild_genmsg() +rosbuild_gensrv() + +include_directories(include) + +rosbuild_add_library(copclientcpp src/cop_client.cpp) + +LINK_LIBRARIES(copclientcpp) +rosbuild_add_executable(test_cop src/test_cop.cpp) +rosbuild_add_executable(refinements src/refinements.cpp) Added: perception/cop_client_cpp/Makefile =================================================================== --- perception/cop_client_cpp/Makefile (rev 0) +++ perception/cop_client_cpp/Makefile 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file Added: perception/cop_client_cpp/include/cop_client_cpp/cop_client.h =================================================================== --- perception/cop_client_cpp/include/cop_client_cpp/cop_client.h (rev 0) +++ perception/cop_client_cpp/include/cop_client_cpp/cop_client.h 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1,56 @@ +#ifndef COP_CLIENT_H +#define COP_CLIENT_H + + +#include <ros/ros.h> +#include <vision_msgs/cop_answer.h> +#include <stdio.h> +#include <tf/tf.h> + + +class CopClient +{ + public: + CopClient(ros::NodeHandle &nh, std::string stTopicName = "/tracking/cop_handler", std::string stCopName = "cop"); + + unsigned long LOFrameQuery(unsigned long child, unsigned long parent); + void LOFreeID(unsigned long id); + double LODistanceQuery(unsigned long child, unsigned long parent); + void LOPointQuery(unsigned long child, float *vector); + unsigned long LONameQuery(std::string name); + tf::Stamped<tf::Pose> LOPoseQuery(unsigned long child); + + unsigned long LOOffset(unsigned long id, unsigned long parent, float x, float y, float z); + + + size_t HasResult(long perception_primitive) + { + if(m_msg_cluster.find(perception_primitive) != m_msg_cluster.end()) + return m_msg_cluster[perception_primitive].size(); + else + return 0; + } + std::vector<vision_msgs::cop_answer> GetResult(long perception_primitive) + { + if(m_msg_cluster.find(perception_primitive) != m_msg_cluster.end()) + return m_msg_cluster[perception_primitive]; + else + return std::vector<vision_msgs::cop_answer> (); + } + + long CallCop(std::string object_class, unsigned long position_id, int num_objects = 1, unsigned long alterantive_id = 0, unsigned long action_type = 0); + long CallCop(std::string object_class, std::vector<unsigned long> position_ids, int num_of_objects, unsigned long alterantive_id, unsigned long action_type); + void CopFeedBack(long primitive, double evaluation, unsigned long error_id); + private: + ros::Subscriber m_readAnswer; + ros::Publisher m_pubFeedBack; + ros::ServiceClient m_client; + ros::ServiceClient m_jlo_client; + std::string m_answerTopic; + std::map<unsigned long, std::vector<vision_msgs::cop_answer> > m_msg_cluster; + + void callback(const boost::shared_ptr<const vision_msgs::cop_answer> &msg); + +}; + +#endif /*COP_CLIENT_H*/ Added: perception/cop_client_cpp/manifest.xml =================================================================== --- perception/cop_client_cpp/manifest.xml (rev 0) +++ perception/cop_client_cpp/manifest.xml 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1,11 @@ +<package> +<description>cop_client_cpp</description> +<author>Ulrich F Klank</author> +<license>GPL</license> +<depend package="roscpp" /> +<depend package="vision_srvs" /> +<depend package="tf" /> +<export> + <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lcopclientcpp" /> +</export> +</package> Added: perception/cop_client_cpp/src/cop_client.cpp =================================================================== --- perception/cop_client_cpp/src/cop_client.cpp (rev 0) +++ perception/cop_client_cpp/src/cop_client.cpp 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1,281 @@ +/* + * Copyright (c) 2009, U. Klank kl...@in... + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <cop_client_cpp/cop_client.h> +#include <vision_srvs/cop_call.h> +#include <vision_msgs/cop_feedback.h> +#include <vision_srvs/srvjlo.h> +using namespace vision_srvs; +using namespace vision_msgs; + +bool breaker = false; +unsigned long vision_primitive = 0; +bool found_trans_obj = false; +unsigned long tobj_pos = 0; + +CopClient::CopClient(ros::NodeHandle &nh, std::string stTopicName, std::string stCopName) +{ + /** subscribe to the topic cop should publish the results*/ + m_answerTopic = stTopicName; + m_readAnswer = nh.subscribe<cop_answer>(stTopicName, 1000, &CopClient::callback, this); + /** Publish */ + char srvname[512]; + sprintf(srvname, "/%s/in", stCopName.c_str()); + ros::service::waitForService(srvname); + m_client = nh.serviceClient<cop_call>(srvname, true); + + ros::service::waitForService("/located_object"); + m_jlo_client = nh.serviceClient<srvjlo>("/located_object", true); + + m_pubFeedBack = nh.advertise<vision_msgs::cop_feedback>("/cop/feedback", 1000); + + + ros::spinOnce(); + +} + +unsigned long CopClient::LOFrameQuery(unsigned long child, unsigned long parent) +{ + srvjlo msg; + msg.request.command = "framequery"; + msg.request.query.id = child; + msg.request.query.parent_id = parent; + m_jlo_client.call(msg); + + return msg.response.answer.id; +} + +void CopClient::LOFreeID(unsigned long id) +{ + srvjlo msg; + msg.request.command = "del"; + msg.request.query.id = id; + m_jlo_client.call(msg); +} + +double CopClient::LODistanceQuery(unsigned long child, unsigned long parent) +{ + srvjlo msg; + if(child == parent) + return 0.0; + msg.request.command = "framequery"; + msg.request.query.id = child; + msg.request.query.parent_id = parent; + m_jlo_client.call(msg); + LOFreeID(msg.response.answer.id); + return sqrt(msg.response.answer.pose[3]*msg.response.answer.pose[3] + msg.response.answer.pose[7]*msg.response.answer.pose[7]+msg.response.answer.pose[11]*msg.response.answer.pose[11]); +} + + +void CopClient::LOPointQuery(unsigned long child, float *vector) +{ + srvjlo msg; + msg.request.command = "framequery"; + msg.request.query.id = child; + msg.request.query.parent_id = 1; + m_jlo_client.call(msg); + + vector[0] = msg.response.answer.pose[3]; + vector[1] = msg.response.answer.pose[7]; + vector[2] = msg.response.answer.pose[11]; + LOFreeID(msg.response.answer.id); + + //return msg.response.answer.pose[11]; +} + + +tf::Stamped<tf::Pose> CopClient::LOPoseQuery(unsigned long child) +{ + srvjlo msg; + msg.request.command = "framequery"; + msg.request.query.id = child; + msg.request.query.parent_id = 1; //map + m_jlo_client.call(msg); + + tf::Stamped<tf::Pose> ret; + ret.frame_id_ = "/map"; + ret.setOrigin(btVector3(msg.response.answer.pose[3],msg.response.answer.pose[7],msg.response.answer.pose[11])); + btMatrix3x3 mat(msg.response.answer.pose[0],msg.response.answer.pose[1],msg.response.answer.pose[2], + msg.response.answer.pose[4],msg.response.answer.pose[5],msg.response.answer.pose[6], + msg.response.answer.pose[8],msg.response.answer.pose[9],msg.response.answer.pose[10]); + btQuaternion qat; + mat.getRotation(qat); + ret.setRotation(qat); + + LOFreeID(msg.response.answer.id); + + //return msg.response.answer.pose[11]; + return ret; +} + + +unsigned long CopClient::LOOffset(unsigned long id, unsigned long parent, float x, float y, float z) +{ + srvjlo msg; + msg.request.command = "update"; + msg.request.query.id = id; + msg.request.query.parent_id = parent; + + msg.request.query.pose[0] = 1.0; + msg.request.query.pose[5] = 1.0; + msg.request.query.pose[10] = 1.0; + msg.request.query.pose[15] = 1.0; + msg.request.query.pose[3] = x; + msg.request.query.pose[7] = y; + msg.request.query.pose[11] = z; + + m_jlo_client.call(msg); + + return msg.response.answer.id; +} + + + +unsigned long CopClient::LONameQuery(std::string name) +{ + vision_srvs::srvjlo msg; + msg.request.command = "namequery"; + msg.request.query.name = name; + if (!m_jlo_client.call(msg)) + { + printf("Error calling jlo!\n"); + } + if (msg.response.error.length() > 0) + { + printf("Error from jlo: %s!\n", msg.response.error.c_str()); + return 0; + } + return msg.response.answer.id; +} + + +void CopClient::callback(const boost::shared_ptr<const cop_answer> &msg) +{ + found_trans_obj = false; + printf("got answer from cop! (Errors: %s)\n", msg->error.c_str()); + m_msg_cluster[msg->perception_primitive].push_back(*msg); + for(size_t i = 0; i < msg->found_poses.size(); i++) + { + const aposteriori_position &pos = msg->found_poses[i]; + printf("Found Obj nr %d with prob %f at pos %d\n", (int)pos.objectId, pos.probability, (int)pos.position); + for(size_t j = 0; j < pos.models.size(); j++) + { + printf(" %s", pos.models[j].sem_class.c_str()); + } + + if(pos.models.size() > 0) + printf("\n"); + } + printf("End!\n"); +} + + +long CopClient::CallCop(std::string object_class, unsigned long position_id, int num_of_objects, unsigned long alterantive_id, unsigned long action_type) +{ + cop_call call; + call.request.outputtopic = m_answerTopic; + + if(alterantive_id == 0 && object_class.length() > 0) + call.request.object_classes.push_back(object_class); + else + call.request.object_ids.push_back(alterantive_id); + + call.request.action_type = action_type; + + call.request.number_of_objects = num_of_objects; + + apriori_position pos; + pos.probability = 1.0; + pos.positionId = position_id; + call.request.list_of_poses.push_back(pos); + + + if(!m_client.call(call)) + { + printf("Error calling cop\n"); + return -1; + } + else + printf("Called cop \n"); + + vision_primitive = call.response.perception_primitive; + return (signed)vision_primitive; +} + + +long CopClient::CallCop(std::string object_class, std::vector<unsigned long> position_ids, int num_of_objects, unsigned long alterantive_id, unsigned long action_type) +{ + cop_call call; + call.request.outputtopic = m_answerTopic; + + if(alterantive_id == 0 && object_class.length() > 0) + call.request.object_classes.push_back(object_class); + else + call.request.object_ids.push_back(alterantive_id); + + call.request.action_type = action_type; + + call.request.number_of_objects = num_of_objects; + for(size_t i = 0 ; i < position_ids.size(); i++) + { + apriori_position pos; + pos.probability = 1.0; + pos.positionId = position_ids[i]; + call.request.list_of_poses.push_back(pos); + } + + + if(!m_client.call(call)) + { + printf("Error calling cop\n"); + return -1; + } + else + printf("Called cop \n"); + + vision_primitive = call.response.perception_primitive; + return (signed)vision_primitive; +} + + + +void CopClient::CopFeedBack(long primitive, double evaluation, unsigned long error_id) +{ + vision_msgs::cop_feedback msg; + msg.evaluation = evaluation; + msg.perception_primitive = primitive; + if(error_id > 0) + { + vision_msgs::system_error err; + err.error_id = error_id; + msg.error.push_back(err); + } + m_pubFeedBack.publish(msg); + +} Added: perception/cop_client_cpp/src/refinements.cpp =================================================================== --- perception/cop_client_cpp/src/refinements.cpp (rev 0) +++ perception/cop_client_cpp/src/refinements.cpp 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1,285 @@ +/* + * Copyright (c) 2009, U. Klank kl...@in... + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <ros/ros.h> +#include <cop_client_cpp/cop_client.h> +#include <std_msgs/String.h> +#include <sstream> + +ros::Subscriber sr; + +std::map<long, std::string> mapping; + +void cb_logging(const std_msgs::StringConstPtr& str) +{ + long test = atoi((*str).data.c_str()); + if(test != 0) + { + mapping[test] = (*str).data; + } +} + +int main(int argc, char* argv[]) +{ + ros::init( argc, argv, "test_cop"); + ros::NodeHandle nh; + CopClient cop(nh); + sr = nh.subscribe("/cop/logging", 1, cb_logging); + + + ros::Rate r(100); + bool refine = false; + bool grasp = true; + bool arm = true; + bool cheap = true; + double xoff = 0.0, yoff = 0.0, zoff = 0.0; + + unsigned long id_base_link = cop.LONameQuery("/base_link"); + unsigned long id_startpos = cop.LOFrameQuery(id_base_link, 1); + unsigned long id_cam = cop.LONameQuery("/RightEyeCalc"); + if(argc > 1) + refine = true; + + unsigned long id_searchspace = cop.LONameQuery("/openni_rgb_optical_frame"); + if(id_searchspace == 0) + id_searchspace = cop.LONameQuery("/narrow_stereo_optical_frame"); + std::string object = "Cluster"; + while(true) + { + long vision_primitive = cop.CallCop(object, id_searchspace, 10); + + + size_t num_results_expected = 1; + + + std::vector<vision_msgs::cop_answer> results; + while(nh.ok()) + { + if(cop.HasResult(vision_primitive) >= num_results_expected) + break; + ros::spinOnce(); + r.sleep(); + } + std::vector<vision_msgs::cop_answer> result = cop.GetResult (vision_primitive); + int j = 0; + int i = 0; + if(result.size() > 0) + { + system("reset"); + printf("============================\n Refine\n============================\n\n"); + for(j = 0; j < result[i].found_poses.size(); j++) + { + printf("%d: Refine %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(), + result[i].found_poses[j].objectId, + result[i].found_poses[j].position); + long vision_primitive = cop.CallCop("", result[i].found_poses[j].position, 1, result[i].found_poses[j].objectId, 256); + printf("Wait for refine\n"); + while(nh.ok()) + { + if(cop.HasResult(vision_primitive) >= num_results_expected) + break; + ros::spinOnce(); + r.sleep(); + } + std::vector<vision_msgs::cop_answer> result_tmp = cop.GetResult (vision_primitive); + + result.push_back(result_tmp[0]); + } + } + for(int bla = 1 ; bla < 2 ; bla++) + for (int trials = 0; trials < 20; trials++) + { + system("reset"); + printf("============================\n Redetect \n============================\n\n"); + int i = 0; + long vision_primitive_ss = cop.CallCop(object, id_searchspace, 10); + size_t num_results_expected_ss = 1; + + + std::vector<vision_msgs::cop_answer> results_ss; + while(nh.ok()) + { + if(cop.HasResult(vision_primitive_ss) >= num_results_expected_ss) + break; + ros::spinOnce(); + r.sleep(); + } + results_ss = cop.GetResult (vision_primitive); + std::vector<unsigned long> vec ; + + for(i = 0; i < results_ss[0].found_poses.size(); i++) + { + vec.push_back(results_ss[0].found_poses[i].position); + } + + for(i = 1; i < result.size(); i++) + { + for(j = 0; j < result[i].found_poses.size(); j++) + { + printf("%d: Redetect %s (%ld at %ld)\n", i, result[i].found_poses[j].models[result[i].found_poses[j].models.size() - 1].sem_class.c_str(), + result[i].found_poses[j].objectId, + result[i].found_poses[j].position); + + } + int select = i; + + if(result.size() > select && result[select].found_poses.size() > 0) + { + long vision_primitive = cop.CallCop("", vec, 1, result[select].found_poses[0].objectId, 0); + printf("Wait for relocate\n"); + while(nh.ok()) + { + if(cop.HasResult(vision_primitive) >= num_results_expected) + break; + ros::spinOnce(); + r.sleep(); + } + std::vector<vision_msgs::cop_answer> result_tmp = cop.GetResult (vision_primitive); + char txt[512]; + bool succes = false; + double dist = 0.0, dist_cam = 0.0; + if(result_tmp.size() > 0 && result_tmp[0].found_poses.size() > 0) + { + dist = cop.LODistanceQuery(result[select].found_poses[0].position, result_tmp[0].found_poses[0].position); + dist_cam = cop.LODistanceQuery(result[select].found_poses[0].position, id_cam); + + succes = true; + } + if(bla > 0) + { + if(dist < dist_cam*0.15) + { + cop.CopFeedBack(vision_primitive, 1.0, 0); + } + else + { + cop.CopFeedBack(vision_primitive, 0.1, 2048); + } + } + std::string text; + if(mapping.find(vision_primitive) != mapping.end()) + text = mapping[vision_primitive]; + sprintf(txt, "%ld %d %f %f %f %f %f %d %s\n", result[select].found_poses[0].objectId, succes ? 1 : 0, dist, xoff, yoff, zoff, dist_cam, bla, text.c_str()); + + FILE *file = fopen("results.txt", "a"); + fwrite(txt, sizeof(*txt), strlen(txt), file); + fclose(file); + } + } +/* if(cheap) + cheap = !cheap;*/ + while(true) + { + /* Move */ + xoff = 0.5 * ((double)rand() / RAND_MAX) - 0.25; + yoff = 0.5 * ((double)rand() / RAND_MAX) - 0.25; + + if(xoff > 0.25) + xoff = 0.25; + if(xoff < -0.25) + xoff = -0.25; + + + if(yoff > 0.25) + yoff = 0.25; + if(yoff < -0.25) + yoff = -0.25; + unsigned long newpos = cop.LOOffset(0, id_base_link, xoff, yoff, zoff); + + printf("GENERATED LO ID %ld\n\n\n", newpos); + double dist = cop.LODistanceQuery(newpos, id_startpos); + if(dist < 0.5) + { + char cmddrive[256]; + sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", newpos); + printf(cmddrive); + system(cmddrive); + break; + } + else + { + printf("Its FAAAR away (dist = %f), ignore\n", dist); + } + cop.LOFreeID(newpos); + } + + } + + while(true) + { + system("reset"); + printf("============================\n END\n============================\n\n"); + + printf("(0) Home & continue\n(1) END \n(2) Toggle arm\n(3) Toggle refine\n(4) Toggle grasp\n(5) Set search space\n(6) Set search term\n(7)continue w/o move"); + + int select2; + int test = scanf("%d", &select2); + printf("Selected %d\n", select2); + if(select2 == 0) + { + char cmddrive[256]; + sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", id_startpos); + system(cmddrive); + break; + } + else if(select2 == 1) + exit(0); + else if(select2 == 2) + arm = !arm; + else if(select2 == 3) + refine = !refine; + else if(select2 == 4) + grasp = !grasp; + else if(select2 == 5) + { + char crash[200]; + test = scanf("%s", &crash); + if(atoi(crash) != 0) + id_searchspace = atoi(crash); + else + { + unsigned long test = cop.LONameQuery(crash); + if(test != 0) + id_searchspace = test; + } + } + else if(select2 == 6) + { + char crash[200]; + test = scanf("%s", &crash); + object = crash; + } + else if (select2 == 7) + { + break; + } + } + } + return 0; +} Added: perception/cop_client_cpp/src/test_cop.cpp =================================================================== --- perception/cop_client_cpp/src/test_cop.cpp (rev 0) +++ perception/cop_client_cpp/src/test_cop.cpp 2011-07-14 01:47:24 UTC (rev 955) @@ -0,0 +1,246 @@ +/* + * Copyright (c) 2009, U. Klank kl...@in... + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <ros/ros.h> +#include <cop_client_cpp/cop_client.h> + +#include <sstream> + +int main(int argc, char* argv[]) +{ + ros::init( argc, argv, "test_cop"); + ros::NodeHandle nh; + CopClient cop(nh); + ros::Rate r(100); + bool refine = false; + bool grasp = true; + bool arm = true; + + unsigned long id_startpos = cop.LONameQuery("/base_link"); + id_startpos = cop.LOFrameQuery(id_startpos, 1); + + if(argc > 1) + refine = true; + + unsigned long id_searchspace = cop.LONameQuery("/openni_rgb_optical_frame"); + if(id_searchspace == 0) + id_searchspace = cop.LONameQuery("/narrow_stereo_optical_frame"); + std::string object = "Cluster"; + while(true) + { + long vision_primitive = cop.CallCop(object, id_searchspace, 10); + + + size_t num_results_expected = 1; + + + std::vector<vision_msgs::cop_answer> results; + while(nh.ok()) + { + if(cop.HasResult(vision_primitive) >= num_results_expected) + break; + ros::spinOnce(); + r.sleep(); + } + std::vector<vision_msgs::cop_answer> result = cop.GetResult (vision_primitive); + int j = 0; + int i = 0; + while(refine && result.size() > 0) + { + system("reset"); + printf("============================\n Refine\n============================\n\n"); + for(j = 0; j < result[i].found_poses.size(); j++) + { + printf("%d: Refine %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(), + result[i].found_poses[j].objectId, + result[i].found_poses[j].position); + } + + printf("%d: %s\n Select cluster to refine: ", j, grasp ? "Continue to grasping": "End"); + int select = result.size(); + int test = scanf("%d", &select); + printf("Selected %d\n", select); + if((unsigned)select < result[i].found_poses.size()) + { + printf("Refine of obj %ld , pos %ld\n", result[i].found_poses[select].objectId, result[i].found_poses[select].position); + long vision_primitive = cop.CallCop("", result[i].found_poses[select].position, 1, result[i].found_poses[select].objectId, 256); + printf("Wait for refine\n"); + while(nh.ok()) + { + if(cop.HasResult(vision_primitive) >= num_results_expected) + break; + ros::spinOnce(); + r.sleep(); + } + } + else + break; + + system("reset"); + printf("============================\n Redetect\n============================\n\n"); + long vision_primitive_ss = cop.CallCop("", result[i].found_poses[select].position, 1, result[i].found_poses[select].objectId); + size_t num_results_expected_ss = 1; + + + std::vector<vision_msgs::cop_answer> results_ss; + while(nh.ok()) + { + if(cop.HasResult(vision_primitive_ss) >= num_results_expected_ss) + break; + ros::spinOnce(); + r.sleep(); + } + results_ss = cop.GetResult (vision_primitive); + for(j = 0; j < result[i].found_poses.size(); j++) + { + printf("%d: Found %s (%ld at %ld)\n", j, results_ss[0].found_poses[j].models[0].sem_class.c_str(), + results_ss[0].found_poses[j].objectId, + results_ss[0].found_poses[j].position); + } + int test10 = scanf("%d", &select); + + } + int result_sys_im = 0; + if(grasp) + { + std::string arm_string = "right"; + if(!arm) + arm_string = "left"; + system("reset"); + printf("============================\n Grasp\n============================\n\n"); + for(j = 0; j < result[i].found_poses.size(); j++) + { + printf("%d: Grasp %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(), + result[i].found_poses[j].objectId, + result[i].found_poses[j].position); + } + + printf("%d: End\n Select cluster to grab: ", j); + int select = result.size(); + int test = scanf("%d", &select); + printf("Selected %d\n", select); + if((unsigned)select < result[i].found_poses.size()) + { + std::ostringstream os; + os << "\""; + for(int k = 0; k < result[i].found_poses.size(); k++) + { + if(k != select) + { + os << result[i].found_poses[k].position << " "; + } + + + } + os << "\""; + + char cmd[1024]; + if( result[i].found_poses.size() > 1) + sprintf(cmd, "rosrun demo_scripts armhand -s %s -r %ld -o %s", arm_string.c_str(), result[i].found_poses[select].position, os.str().c_str()); + else + sprintf(cmd, "rosrun demo_scripts armhand -s %s -r %ld", arm_string.c_str(), result[i].found_poses[select].position); + printf("Calling %s\n", cmd); + result_sys_im = system(cmd); + printf("\n\nSystem result: %d\n\n", result_sys_im); + sleep(0.5); + int result_sys; + if(result_sys_im == 0 || 2560 == result_sys_im) + { + sprintf(cmd, "rosrun demo_scripts armhand -s %s -l 0.2", arm_string.c_str()); + result_sys = system(cmd); + sleep(1.5); + + sprintf(cmd, "rosrun demo_scripts armhand -s %s -l -0.2", arm_string.c_str()); + result_sys = system(cmd); + sleep(1.0); + sprintf(cmd, "rosrun demo_scripts armhand -s %s -g open_relax", arm_string.c_str()); + result_sys = system(cmd); + sleep(0.5); + sprintf(cmd, "rosrun demo_scripts armhand -s %s -l 0.3", arm_string.c_str()); + result_sys = system(cmd); + sleep(1.0); + } + sprintf(cmd, "rosrun demo_scripts armhand -s %s -p open", arm_string.c_str()); + result_sys = system(cmd); + sleep(0.1); + } + } + while(true) + { + system("reset"); + printf("============================\n END\n============================\n\n"); + printf("\n\nResult of grasp was %d\n\n", result_sys_im); + + printf("(0) Home & continue\n(1) END \n(2) Toggle arm\n(3) Toggle refine\n(4) Toggle grasp\n(5) Set search space\n(6) Set search term\n(7)continue w/o move"); + + int select2; + int test = scanf("%d", &select2); + printf("Selected %d\n", select2); + if(select2 == 0) + { + char cmddrive[256]; + sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", id_startpos); + system(cmddrive); + break; + } + else if(select2 == 1) + exit(0); + else if (select2 == 2) + arm = !arm; + else if(select2 == 3) + refine = !refine; + else if(select2 == 4) + grasp = !grasp; + else if(select2 == 5) + { + char crash[200]; + test = scanf("%s", &crash); + if(atoi(crash) != 0) + id_searchspace = atoi(crash); + else + { + unsigned long test = cop.LONameQuery(crash); + if(test != 0) + id_searchspace = test; + } + } + else if(select2 == 6) + { + char crash[200]; + test = scanf("%s", &crash); + object = crash; + } + else if (select2 == 7) + { + break; + } + } + } + return 0; +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-06-17 05:10:50
|
Revision: 952 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=952&view=rev Author: moesenle Date: 2011-06-17 05:10:44 +0000 (Fri, 17 Jun 2011) Log Message: ----------- Author: Dejan Pangercic <dej...@cs...> Date: Thu Jun 16 22:10:09 2011 -0700 commit 48d35e71faea2e1149a349750165f27df73d61d9 Author: Dejan Pangercic <dej...@cs...> Date: Thu Jun 16 22:07:19 2011 -0700 added a hack parameter to disable distance keeping. For tables on legs (no solid frontal surfaces) the current version of nav_pcontroller results in too jerky motions while in the zone of approaching the target commit 2d52c11597cb2ee400a3b69e2330ba415540cf42 Author: Ingo Kresse <kr...@in...> Date: Wed Apr 20 16:03:09 2011 +0200 renamed global_frame parameter to odom_frame (makes more sense) commit 0cf6c2b5e4325d75483668f6af2e1e6d7fd2b1cb Author: Ingo Kresse <kr...@in...> Date: Wed Apr 20 15:56:57 2011 +0200 made nav_pcontroller frames a ROS param (should work on pr2 again) commit d0b9ce4e6312693f861728f5fd5a40d39f225028 Author: Ingo Kresse <kr...@in...> Date: Tue Apr 19 16:05:15 2011 +0200 nav_pcontroller: (finally) renamed variables top and bottom commit b4690ad0fa1a0b241c687f3133f5678248fd03af Author: Ingo Kresse <kr...@in...> Date: Wed Apr 13 19:07:23 2011 +0200 improved nav_pcontroller, works in simulation (tm) - a lot of cleanups - store in odom frame -> accounts for different laser timestamps -> control rate now depends on tf rate rather than laser freqency - throw away far away points (saves a lot computation time) - do 3-point derivative on distance function (waste some computation time, also adjusted derivation step to current velocity) - uses now-avaliable second derivative to "dampen" first derivative (gets rid of instability, yippie!) commit 6670fb06f34a71fcef1d0bca895799dc5be18565 Author: Alexis Maldonado <mal...@tu...> Date: Sun Oct 31 19:33:43 2010 +0100 nav_pcontroller: Fix a typo so it uses the correct frame for navigating commit 869b5dea5b32890a59988c87b49f4983f5d84d53 Author: Lorenz Moesenlechner <moe...@in...> Date: Sun Oct 31 17:30:52 2010 +0100 Re-export of nav_pcontroller git-commit Modified Paths: -------------- stacks/ias_nav/nav_pcontroller/src/nav_pcontroller.cc Modified: stacks/ias_nav/nav_pcontroller/src/nav_pcontroller.cc =================================================================== --- stacks/ias_nav/nav_pcontroller/src/nav_pcontroller.cc 2011-06-08 08:17:21 UTC (rev 951) +++ stacks/ias_nav/nav_pcontroller/src/nav_pcontroller.cc 2011-06-17 05:10:44 UTC (rev 952) @@ -106,7 +106,7 @@ double vx_, vy_, vth_; double x_goal_, y_goal_, th_goal_; double x_now_, y_now_, th_now_; - bool goal_set_; + bool goal_set_, keep_distance_; std::string global_frame_; std::string base_link_frame_; @@ -253,6 +253,7 @@ n_.param<double>("acc_lin_max", acc_lin_max_, 0.4); n_.param<int>("loop_rate", loop_rate_, 30); n_.param<double>("p", p_, 1.2); + n_.param<bool>("keep_distance", keep_distance_, true); #ifdef JLO_BASE_CONTROL n_.param<bool>("enable_jlo", jlo_enabled_, true); @@ -523,9 +524,10 @@ } compute_p_control(); + + if (keep_distance_) + dist_control_.compute_distance_keeping(&vx_, &vy_, &vth_); - dist_control_.compute_distance_keeping(&vx_, &vy_, &vth_); - sendVelCmd(vx_, vy_, vth_); if(comparePoses(x_goal_, y_goal_, th_goal_, x_now_, y_now_, th_now_)) { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <tum...@li...> - 2011-06-08 08:17:27
|
Revision: 951 http://tum-ros-pkg.svn.sourceforge.net/tum-ros-pkg/?rev=951&view=rev Author: moesenle Date: 2011-06-08 08:17:21 +0000 (Wed, 08 Jun 2011) Log Message: ----------- Author: Lars Kunze <ku...@in...> Date: Wed Jun 8 16:33:32 2011 +0900 commit b4d1d6e61c1c30e0133ecaf1c81eed6f2f7ecaa5 Author: Lars Kunze <ku...@in...> Date: Wed Jun 8 16:16:17 2011 +0900 Added support for handling more object types and target IRI (namespace) for the generated map. Basically all object types that are subclasses of SpatialThing-Localized can be used now. Since Knowrob's ontology is parsed to find all these subclasses, now the package depends on ias_knowledge_base. The following command will print out a list of all valid object types: % rosrun mod_semantic_map SemanticMapToOWL list Please also note that object types are case-insensitive now, i.e., door, Door, DOOR will work. The field frame_id in the Header msg can now be used for an IRI of the resulting map, e.g. 'http://www.example.com/my_map.owl#' git-commit Modified Paths: -------------- knowledge/mod_semantic_map/manifest.xml knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java Modified: knowledge/mod_semantic_map/manifest.xml =================================================================== --- knowledge/mod_semantic_map/manifest.xml 2011-06-01 13:33:44 UTC (rev 950) +++ knowledge/mod_semantic_map/manifest.xml 2011-06-08 08:17:21 UTC (rev 951) @@ -13,6 +13,7 @@ <url>http://ros.org/wiki/mod_semantic_map</url> <depend package="rosjava" /> <depend package="geometry_msgs" /> + <depend package="ias_knowledge_base" /> </package> Modified: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java 2011-06-01 13:33:44 UTC (rev 950) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWL.java 2011-06-08 08:17:21 UTC (rev 951) @@ -1,16 +1,20 @@ package edu.tum.cs.ias.knowrob; import java.io.*; +import java.util.Collection; import java.util.Date; import java.util.ArrayList; import java.util.HashMap; import java.text.SimpleDateFormat; + import org.semanticweb.owlapi.apibinding.OWLManager; import org.semanticweb.owlapi.io.RDFXMLOntologyFormat; import org.semanticweb.owlapi.model.*; import org.semanticweb.owlapi.util.DefaultPrefixManager; import org.semanticweb.owlapi.vocab.PrefixOWLOntologyFormat; +import org.semanticweb.owlapi.reasoner.*; +import org.semanticweb.owlapi.reasoner.structural.*; import ros.*; import ros.pkg.mod_semantic_map.srv.*; @@ -26,6 +30,7 @@ * as to retrieve object models from the database and add them to CoP. * * @author Moritz Tenorth, te...@cs... + * @author Lars Kunze, ku...@cs... * */ @@ -47,43 +52,49 @@ // Base IRI for semantic map ontology public final static String IAS_MAP = "http://ias.cs.tum.edu/kb/ccrl2_semantic_map.owl#"; + + // Ros package name for KnowRob + public final static String KNOWROB_PKG = "ias_knowledge_base"; + // OWL file of the KnowRob ontology (relative to KNOWROB_PKG) + public final static String KNOWROB_OWL = "owl/knowrob.owl"; // Prefix manager public final static DefaultPrefixManager PREFIX_MANAGER = new DefaultPrefixManager(IAS_MAP); static { PREFIX_MANAGER.setPrefix("knowrob:", KNOWROB); PREFIX_MANAGER.setPrefix("ias_map:", IAS_MAP); - PREFIX_MANAGER.setPrefix("owl:", OWL); + PREFIX_MANAGER.setPrefix("owl:", OWL); PREFIX_MANAGER.setPrefix("rdfs:", RDFS); } // mapping ROS-KnowRob identifiers protected static final HashMap<String, String> rosToKnowrob = new HashMap<String, String>(); - static { - rosToKnowrob.put("cupboard", "knowrob:Cupboard"); - rosToKnowrob.put("drawer", "knowrob:Drawer"); - rosToKnowrob.put("oven", "knowrob:Oven"); - rosToKnowrob.put("refrigerator", "knowrob:Refrigerator"); - rosToKnowrob.put("dishwasher", "knowrob:Dishwasher"); - rosToKnowrob.put("table", "knowrob:Table"); - rosToKnowrob.put("countertop", "knowrob:CounterTop"); - rosToKnowrob.put("sink", "knowrob:Sink"); - rosToKnowrob.put("door", "knowrob:Door"); - rosToKnowrob.put("hinge", "knowrob:HingedJoint"); - rosToKnowrob.put("handle", "knowrob:Handle"); - rosToKnowrob.put("knob", "knowrob:ControlKnob"); + // static { + // rosToKnowrob.put("cupboard", "knowrob:Cupboard"); + // rosToKnowrob.put("drawer", "knowrob:Drawer"); + // rosToKnowrob.put("oven", "knowrob:Oven"); + // rosToKnowrob.put("refrigerator", "knowrob:Refrigerator"); + // rosToKnowrob.put("dishwasher", "knowrob:Dishwasher"); + // rosToKnowrob.put("table", "knowrob:Table"); + // rosToKnowrob.put("countertop", "knowrob:CounterTop"); + // rosToKnowrob.put("sink", "knowrob:Sink"); + // rosToKnowrob.put("door", "knowrob:Door"); + // rosToKnowrob.put("hinge", "knowrob:HingedJoint"); + // rosToKnowrob.put("handle", "knowrob:Handle"); + // rosToKnowrob.put("knob", "knowrob:ControlKnob"); - // TODO: add a concept for horizontal planes to knowrob - rosToKnowrob.put("horizontal_plane", "knowrob:CounterTop"); - } + // // TODO: add a concept for horizontal planes to knowrob + // rosToKnowrob.put("horizontal_plane", "knowrob:CounterTop"); + // } HashMap<Integer, OWLNamedIndividual> idToInst; OWLDataFactory factory; OWLOntologyManager manager; DefaultPrefixManager pm; - + + int inst_counter=0; // counter to create unique instance identifiers //////////////////////////////////////////////////////////////////////////////// @@ -104,8 +115,10 @@ * @param n the node handle * @throws RosException if advertising ROS services failed */ - public SemanticMapToOWL() { + public SemanticMapToOWL() { + initRosToKnowrob(); + idToInst = new HashMap<Integer, OWLNamedIndividual>(); try { @@ -120,7 +133,69 @@ } } + + /** + * Initialization of the mapping between object types that are send via + * the ros service and concepts of the KnowRob ontology. + */ + protected static void initRosToKnowrob() { + + //OWLOntology ont = manager.loadOntology(IRI.create(KNOWROB)); + try { + // Create ontology manager, data factory, and prefix manager + OWLOntologyManager manager = OWLManager.createOWLOntologyManager(); + OWLDataFactory factory = manager.getOWLDataFactory(); + DefaultPrefixManager pm = PREFIX_MANAGER; + + // Find ros package holding the knowrob ontology + String knowrob_pkg = rospackFind(KNOWROB_PKG); + String knowrob_owl = knowrob_pkg + "/" + KNOWROB_OWL; + + //System.out.println("Path to Knowrob: " + knowrob_owl); + + // Load the knowrob ontology + OWLOntology ont = manager.loadOntologyFromOntologyDocument(new File(knowrob_owl)); + + // Retrieve only subclasses of SpatialThing-Localized + OWLReasoner reasoner = new StructuralReasoner(ont, new SimpleConfiguration(), BufferingMode.NON_BUFFERING); + OWLClass spatialThing = factory.getOWLClass("knowrob:SpatialThing-Localized", pm); + NodeSet<OWLClass> ns = reasoner.getSubClasses(spatialThing, false); + + java.util.Set<Node<OWLClass>> set = ns.getNodes(); + + // Iterate over all subclasses and put them into the mapping hashmap + for(Node n : set) { + OWLClass c = (OWLClass) n.getRepresentativeElement(); + + String iri = c.toStringID().replaceAll(KNOWROB, "knowrob:"); + String key = c.toStringID().substring(c.toStringID().lastIndexOf('#') + 1).toLowerCase(); + + //System.out.println(key + " - " + iri); + rosToKnowrob.put(key, iri); + } + // to support backward compability (should be removed) + rosToKnowrob.put("hinge", "knowrob:HingedJoint"); + rosToKnowrob.put("knob", "knowrob:ControlKnob"); + rosToKnowrob.put("horizontal_plane", "knowrob:CounterTop"); + + //System.out.println("Number of supported object types: " + rosToKnowrob.size()); + } + catch (Exception e) { + e.printStackTrace(); + } + } + + protected static void printObjectTypes() { + + Collection c = rosToKnowrob.values(); + + for(Object o : c) { + System.out.println(((String)o).replaceAll("knowrob:","")); + } + + } + /** * Thread-safe ROS initialization @@ -174,7 +249,18 @@ OWLOntology ontology = null; idToInst.clear(); try { - + + // Get iri of target map from the frame_id of header msg + String map_id = ros_map.header.frame_id.toString(); + if (map_id.length() != 0) { + // use provided map_id + PREFIX_MANAGER.setPrefix("ias_map:", map_id); + } else { + //use IAS_MAP as default, PREFIX_MANAGER ist set by default + map_id = IAS_MAP; + } + System.out.println("Using map id: " + map_id); + // Create ontology manager and data factory manager = OWLManager.createOWLOntologyManager(); factory = manager.getOWLDataFactory(); @@ -183,7 +269,7 @@ pm = PREFIX_MANAGER; // Create empty OWL ontology - ontology = manager.createOntology(IRI.create(IAS_MAP)); + ontology = manager.createOntology(IRI.create(map_id)); manager.setOntologyFormat(ontology, new RDFXMLOntologyFormat()); // Import KnowRob ontology @@ -191,21 +277,19 @@ AddImport addImp = new AddImport(ontology,oid); manager.applyChange(addImp); - // create SemanticMap object in the ontology createSemMapInst(ontology); // create time point OWLNamedIndividual time_inst = createTimePointInst(ros_map.header.stamp, ontology); - - + // iterate over all objects and create the respective OWL representations for(SemMapObject ros_obj : ros_map.objects) { createObjectDescription(ros_obj, time_inst, ontology); } SimpleDateFormat sdf = new SimpleDateFormat("yy-MM-dd_HH-mm-ss-SSS"); - String outfile = "ias_semantic_map_"+sdf.format(new Date())+".owl"; + String outfile = "semantic_map_"+sdf.format(new Date())+".owl"; saveOntologyToFile(ontology, outfile); } catch (Exception e) { @@ -289,11 +373,12 @@ protected OWLNamedIndividual createObjectInst(SemMapObject ros_obj, OWLOntology ontology) { // create object instance - OWLClass obj_class = factory.getOWLClass(rosToKnowrob.get(ros_obj.type), pm); + OWLClass obj_class = factory.getOWLClass(rosToKnowrob.get(ros_obj.type.toLowerCase()), pm); OWLNamedIndividual obj_inst = factory.getOWLNamedIndividual( - instForClass(rosToKnowrob.get(ros_obj.type)), pm); - manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(obj_class, obj_inst)); + instForClass(rosToKnowrob.get(ros_obj.type.toLowerCase())), pm); + manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(obj_class, obj_inst)); + // set object dimensions OWLDataProperty width = factory.getOWLDataProperty("knowrob:widthOfObject", pm); OWLDataProperty depth = factory.getOWLDataProperty("knowrob:depthOfObject", pm); @@ -310,22 +395,22 @@ if(parent!=null) { // select linking property based on the object type - if(ros_obj.type.equals("cupboard") || - ros_obj.type.equals("drawer") || - ros_obj.type.equals("oven") || - ros_obj.type.equals("refrigerator") || - ros_obj.type.equals("dishwasher") || - ros_obj.type.equals("table") || - ros_obj.type.equals("countertop") || - ros_obj.type.equals("horizontal_plane") || - ros_obj.type.equals("sink")) { - + if(ros_obj.type.equalsIgnoreCase("cupboard") || + ros_obj.type.equalsIgnoreCase("drawer") || + ros_obj.type.equalsIgnoreCase("oven") || + ros_obj.type.equalsIgnoreCase("refrigerator") || + ros_obj.type.equalsIgnoreCase("dishwasher") || + ros_obj.type.equalsIgnoreCase("table") || + ros_obj.type.equalsIgnoreCase("countertop") || + ros_obj.type.equalsIgnoreCase("horizontal_plane") || + ros_obj.type.equalsIgnoreCase("sink")) { + // top-level object, link to map OWLObjectProperty describedInMap = factory.getOWLObjectProperty("knowrob:describedInMap", pm); manager.addAxiom(ontology, factory.getOWLObjectPropertyAssertionAxiom(describedInMap, obj_inst, parent)); - } else if(ros_obj.type.equals("door")) { + } else if(ros_obj.type.equalsIgnoreCase("door")) { // doors are both part of parent and hinged to it OWLObjectProperty properPhysicalParts = factory.getOWLObjectProperty("knowrob:properPhysicalParts", pm); @@ -366,7 +451,7 @@ instForClass("ias_map:RotationMatrix3D"), pm); manager.addAxiom(ontology, factory.getOWLClassAssertionAxiom(pose_class, pose_inst)); - System.out.println("\n\n"+ros_obj.id + " " + ros_obj.type); + System.out.println("\n\n"+ros_obj.id + " " + ros_obj.type + " - " + rosToKnowrob.get(ros_obj.type.toLowerCase())); // set pose properties for(int i=0;i<4;i++) { @@ -421,7 +506,7 @@ * @return Instance identifier (class string plus index) */ protected String instForClass(String cl) { - cl.replaceAll("knorob", "ias_map"); + cl = cl.replaceAll("knowrob", "ias_map"); return cl+(inst_counter++); } @@ -587,6 +672,33 @@ return ok; } + /** + * Finds a ros package using rospack and returns its path. + * @param name of the ros package + * @return path to the package - if it was found<br> + * <tt>null</tt> - otherwise + */ + public static String rospackFind(String pkg) { + + String path = null; + try + { + Process p = Runtime.getRuntime().exec("rospack find " + pkg); + p.waitFor(); + BufferedReader br = new BufferedReader(new InputStreamReader(p.getInputStream())); + + if ((path = br.readLine()) != null){ + ;//System.out.println("Package: " + pkg + ", Path: " + path); + } + else + ;//System.out.println("Package: " + pkg + ", Error: package not found!"); + } + catch (Exception e) + { + e.printStackTrace(); + } + return path; + } // @@ -597,9 +709,22 @@ public static void main(String[] args) { - - new SemanticMapToOWL(); - + + + if (args.length == 0) { + // run service + new SemanticMapToOWL(); + } else if (args.length == 1 && args[0].equals("list")){ + initRosToKnowrob(); + printObjectTypes(); + } else { + System.out.println("usage: rosrun mod_semantic_map SemanticMapToOWL"); + System.out.println("Commands:"); + System.out.println(" rosrun mod_semantic_map SemanticMapToOWL Runs the service"); + System.out.println(" rosrun mod_semantic_map SemanticMapToOWL list Show supported object types"); + System.out.println(); + } + } Modified: knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java =================================================================== --- knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java 2011-06-01 13:33:44 UTC (rev 950) +++ knowledge/mod_semantic_map/src/edu/tum/cs/ias/knowrob/SemanticMapToOWLTestClient.java 2011-06-08 08:17:21 UTC (rev 951) @@ -44,13 +44,14 @@ GenerateSemanticMapOWL.Request req = new GenerateSemanticMapOWL.Request(); req.map= new SemMap(); - req.map.header.frame_id="/map"; + // Set the IRI for the map that will be created + req.map.header.frame_id="http://www.example.com/foo.owl#"; req.map.header.stamp=ros.now(); // req.map.objects = new SemMapObject[4]; // create cupboard - req.map.objects.add(new SemMapObject()); + req.map.objects.add(new SemMapObject()); req.map.objects.get(0).id=1; req.map.objects.get(0).partOf=0; req.map.objects.get(0).type="cupboard"; @@ -67,7 +68,7 @@ req.map.objects.add(new SemMapObject()); req.map.objects.get(1).id=2; req.map.objects.get(1).partOf=1; - req.map.objects.get(1).type="door"; + req.map.objects.get(1).type="Door"; req.map.objects.get(1).depth = 0.6f; req.map.objects.get(1).width = 60.7f; @@ -81,7 +82,7 @@ req.map.objects.add(new SemMapObject()); req.map.objects.get(2).id=3; req.map.objects.get(2).partOf=2; - req.map.objects.get(2).type="hinge"; + req.map.objects.get(2).type="HINGEDJOINT"; req.map.objects.get(2).depth = 5.6f; req.map.objects.get(2).width = 0.7f; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |