Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2963] pkg/trunk
|
From: <adv...@us...> - 2008-08-12 21:54:11
|
Revision: 2963
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2963&view=rev
Author: advaitjain
Date: 2008-08-12 21:54:20 +0000 (Tue, 12 Aug 2008)
Log Message:
-----------
overhead_grasp_behavior uses gmmseg to segment the object,
find orientation and refine object's position.
fixed some minor bugs in gmmseg.
Modified Paths:
--------------
pkg/trunk/manip/overhead_grasp_behavior/CMakeLists.txt
pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
pkg/trunk/manip/overhead_grasp_behavior/overhead_grasp.xml
pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/util/pyrob/src/pyrob/util.py
pkg/trunk/vision/gmmseg/manifest.xml
pkg/trunk/vision/gmmseg/nodes/gmm_client.py
pkg/trunk/vision/gmmseg/nodes/gmm_segment.py
Modified: pkg/trunk/manip/overhead_grasp_behavior/CMakeLists.txt
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/CMakeLists.txt 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/manip/overhead_grasp_behavior/CMakeLists.txt 2008-08-12 21:54:20 UTC (rev 2963)
@@ -1,8 +1,10 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
add_definitions(-Wall)
+
rospack(overhead_grasp_behavior)
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-rospack_add_executable(bin/overhead_grasp src/overhead_grasp.cc)
+rospack_add_executable(overhead_grasp src/overhead_grasp.cc)
Modified: pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-12 21:54:20 UTC (rev 2963)
@@ -10,4 +10,5 @@
<depend package="libpr2API"/>
<depend package="pr2_gazebo"/>
<depend package="pr2_kinematic_controllers"/>
+<depend package="gmmseg" />
</package>
Modified: pkg/trunk/manip/overhead_grasp_behavior/overhead_grasp.xml
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/overhead_grasp.xml 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/manip/overhead_grasp_behavior/overhead_grasp.xml 2008-08-12 21:54:20 UTC (rev 2963)
@@ -6,6 +6,7 @@
<node pkg="pr2_gazebo" type="run-pr2_gazebo.sh" args="" respawn="true" />
<node pkg="pr2_kinematic_controllers" type="pr2_kinematic_controllers" respawn="true" />
<node pkg="overhead_grasp_behavior" type="overhead_grasp" respawn="true" />
+ <node pkg="gmmseg" type="gmm_segment.py" args="image:=image_wrist_right" respawn="true" />
</group>
</launch>
Modified: pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-12 21:54:20 UTC (rev 2963)
@@ -16,7 +16,9 @@
#include <rosgazebo/GripperCmd.h>
#include <std_msgs/PR2Arm.h>
+#include <gmmseg/hrl_grasp.h>
+
using namespace KDL;
class OverheadGrasper : public ros::node
@@ -28,7 +30,7 @@
pr2_msgs::EndEffectorState rightEndEffectorMsg;
Frame right_tooltip_frame;
Vector objectPosition;
- const static double PR2_GRIPPER_LENGTH = 0.155;
+ const static double PR2_GRIPPER_LENGTH = 0.16;
Vector CAMERA_ENDEFFECTOR; // position of camera relative to end effector in end effector frame.
public:
@@ -113,20 +115,25 @@
void moveArmSegmentation()
{
-// gmmseg::hrl_grasp:request req;
-// gmmseg::hrl_grasp:response res;
-// req.height = right_tooltip_frame.p[2] - objectPosition[2] - CAMERA_ENDEFFECTOR[0];
-// if (ros::service::call("hrl_grasp", req, res)==false)
-// {
-// printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {segmentObject} hrl_grasp service failed.\nExiting..\n");
-// exit(0);
-// }
-// Vector move(-1*res.y, -1*res.x, 0);
-// Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotX(res.theta); // look down vertically, with correct wrist roll
+ gmmseg::hrl_grasp::request req;
+ gmmseg::hrl_grasp::response res;
+ req.height = right_tooltip_frame.p[2] - objectPosition[2] - CAMERA_ENDEFFECTOR[0];
+ if (ros::service::call("hrl_grasp", req, res)==false)
+ {
+ printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {segmentObject} hrl_grasp service failed.\nExiting..\n");
+ exit(0);
+ }
+ Vector move(-1*res.y, -1*res.x, 0);
+ cout<<"move: "<<move<<"\n";
- Vector move(0,0,0);
move += Rotation::RotY(deg2rad*90)*CAMERA_ENDEFFECTOR;
- Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotY(deg2rad*90); // look down vertically, with correct wrist roll
+
+ if (res.theta>0)
+ res.theta = deg2rad*90-res.theta;
+ else
+ res.theta = -1*(res.theta+deg2rad*90);
+
+ Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotY(deg2rad*90)*Rotation::RotZ(res.theta); // look down vertically, with correct wrist roll
Vector goto_point = right_tooltip_frame.p+move;
cout<<"Going to: "<<goto_point<<endl;
positionArmCartesian(goto_point, r);
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-08-12 21:54:20 UTC (rev 2963)
@@ -151,18 +151,18 @@
<!-- The small box "cup" -->
<model:physical name="object1_model">
<xyz> 0.620 -0.45 0.65</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
+ <rpy> 0.0 0.0 -30.0</rpy>
<body:box name="object1_body">
<geom:box name="object1_geom">
<kp>100000000.0</kp>
<kd>0.1</kd>
<mesh>default</mesh>
- <size>0.1 0.03 0.03</size>
+ <size>0.1 0.03 0.06</size>
<mass> 0.05</mass>
<mu1> 500.0 </mu1>
<mu2> 500.0 </mu2>
<visual>
- <size> 0.1 0.030 0.03</size>
+ <size> 0.1 0.030 0.06</size>
<material>Gazebo/PioneerBody</material>
<mesh>unit_box</mesh>
</visual>
Modified: pkg/trunk/util/pyrob/src/pyrob/util.py
===================================================================
--- pkg/trunk/util/pyrob/src/pyrob/util.py 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/util/pyrob/src/pyrob/util.py 2008-08-12 21:54:20 UTC (rev 2963)
@@ -159,12 +159,10 @@
cv.CV_8UC4 : (np.uint8, 4)}
def ros2cv(image):
- if image.colorspace == 'rgb24':
- format = 'RGB'
- elif image.colorspace == 'mono8':
+ if image.colorspace == 'mono8':
format = 'L'
else:
- raise RuntimeError('ros2cv: invalid colorspace in ROS image\'s im.colorspace.')
+ format = 'RGB'
pil_image = Image.fromstring(format, (image.width, image.height), image.data)
pil_image.save('ros2cv.bmp', 'BMP')
Modified: pkg/trunk/vision/gmmseg/manifest.xml
===================================================================
--- pkg/trunk/vision/gmmseg/manifest.xml 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/vision/gmmseg/manifest.xml 2008-08-12 21:54:20 UTC (rev 2963)
@@ -10,5 +10,8 @@
<depend package="pyrob"/>
<depend package="numpy"/>
<depend package="rospy"/>
- <depend package="std_msgs"/>
+ <depend package="std_msgs"/>
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp" />
+ </export>
</package>
Modified: pkg/trunk/vision/gmmseg/nodes/gmm_client.py
===================================================================
--- pkg/trunk/vision/gmmseg/nodes/gmm_client.py 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/vision/gmmseg/nodes/gmm_client.py 2008-08-12 21:54:20 UTC (rev 2963)
@@ -10,8 +10,8 @@
import rospy
import sys
-rospy.wait_for_service('gmm_segment')
-segment_proxy = rospy.ServiceProxy('gmm_segment', srv.hrl_grasp)
-result = segment_proxy.call(srv.hrl_graspRequest(float(sys.argv[0])))
+rospy.wait_for_service('hrl_grasp')
+segment_proxy = rospy.ServiceProxy('hrl_grasp', srv.hrl_grasp)
+result = segment_proxy.call(srv.hrl_graspRequest(float(sys.argv[1])))
print result.x, result.y, result.z, result.theta
Modified: pkg/trunk/vision/gmmseg/nodes/gmm_segment.py
===================================================================
--- pkg/trunk/vision/gmmseg/nodes/gmm_segment.py 2008-08-12 21:51:50 UTC (rev 2962)
+++ pkg/trunk/vision/gmmseg/nodes/gmm_segment.py 2008-08-12 21:54:20 UTC (rev 2963)
@@ -1,3 +1,4 @@
+#!/usr/bin/python
'''
@package gmm_segment
Provides Gaussian mixture segmentation service.
@@ -13,6 +14,7 @@
import pyrob.util as ut
import sys, time, math
import opencv.highgui as hg
+import time
CAMERA_FOV = 60.0
CAMERA_WIDTH = 640
@@ -27,19 +29,40 @@
self.fy = CAMERA_HEIGHT / t30
def segment(self, request):
+ print 'Hey Advait, I want to segment the image'
while self.image == None:
time.sleep(0.1)
self.lock.acquire()
- sego = tg.segment_center_object(self.image)
+ sego = tg.segment_center_object(self.image, display_on=False)
self.lock.release()
z = request.height
- u = sego.fg_object_ellipse.center[0]
- v = sego.fg_object_ellipse.center[1]
+ u = sego.fg_object_ellipse.center[0] - (CAMERA_WIDTH / 2.0)
+ v = sego.fg_object_ellipse.center[1] - (CAMERA_HEIGHT / 2.0)
x = u * z / self.fx
y = v * z / self.fy
theta = sego.fg_object_ellipse.angle
+
+ if theta > 90:
+ theta = theta - 180
+ if theta < -90:
+ theta = theta + 180
+
+ theta = math.radians(theta)
+
+# if True:
+# image_list = sego.get_images_for_display()
+# image_list.append( cvimg )
+#
+# curtime = time.localtime()
+# date_name = time.strftime('%Y%m%d%I%M', curtime)
+#
+# for i,img in enumerate(image_list):
+# fname = date_name+'_image%d.png'%(i)
+# hg.cvSaveImage( fname, img )
+
+
return srv.hrl_graspResponse(x, y, request.height, theta)
def set_image(self, image):
@@ -55,7 +78,7 @@
rospy.TopicSub('image', RImage, sn.set_image)
rospy.ready(sys.argv[0])
- segment_service = rospy.Service('gmm_segment', srv.hrl_grasp, sn.segment)
+ segment_service = rospy.Service('hrl_grasp', srv.hrl_grasp, sn.segment)
segment_service.register()
rospy.spin()
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
