Skip to content

Commit 529ecbc

Browse files
pidipidipidipidi
committed
First commit
0 parents  commit 529ecbc

25 files changed

+2629
-0
lines changed

‎.gitignore

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
*.pkl
2+
*.gz

‎CMakeLists.txt

+39
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
cmake_minimum_required(VERSION 2.8.3)
2+
project(object_estimator)
3+
4+
find_package(catkin
5+
REQUIRED
6+
COMPONENTS
7+
rospy
8+
geometry_msgs
9+
message_generation
10+
)
11+
12+
catkin_python_setup()
13+
14+
add_service_files(
15+
FILES None_String.srv
16+
)
17+
18+
add_message_files(
19+
FILES EndpointState.msg
20+
)
21+
22+
generate_messages(
23+
DEPENDENCIES geometry_msgs
24+
)
25+
26+
27+
catkin_package(
28+
CATKIN_DEPENDS
29+
rospy
30+
geometry_msgs
31+
message_runtime
32+
)
33+
34+
35+
install(
36+
DIRECTORY launch/
37+
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
38+
USE_SOURCE_PERMISSIONS
39+
)

‎Makefile

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
init:
2+
pip install -r requirements.txt
3+
4+
test:
5+
py.test tests
6+
7+
.PHONY: init test

‎README.md

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
1+
# Object_estimator
2+
This repository is intended for a package required to estimate the internal state of a grasped object by a manipulator. This includes a binary classifier using log-likelihood ratio from hidden Markov models (HMMs), a data preprocessor, and a topic recording script.
3+
4+
This repository is tested with a Baxter robot in RRG, MIT. For deployment, users have to record a set of training data rosbags, train an HMM, and run a ros-service with data topics.
5+
6+
## Installation
7+
- Install HMM library following http://ghmm.sourceforge.net/installation.html
8+
- Install dependencies
9+
~~~~bash
10+
pip install -r requirements.txt
11+
~~~~
12+
- Compile this package
13+
~~~~bash
14+
catkin_make
15+
~~~~
16+
- Download test data
17+
~~~~bash
18+
roscd object_estimator
19+
cd data
20+
wget https://www.dropbox.com/s/vj8g3ftplq0lru6/object_estimator.tar.gz
21+
tar -xvzf object_estimator.tar.gz
22+
~~~~
23+
24+
## Test
25+
- Preprocess the collected data.
26+
~~~~bash
27+
roslaunch object_estimator full_cup_estimation.launch en_preprocess:=true plot:=true
28+
~~~~
29+
30+
- Then, run the estimation service node. You can see a service node "/object_state/left". You can also specify a service node name in the launch file.
31+
~~~~bash
32+
roslaunch object_estimator full_cup_estimation.launch
33+
~~~~
34+
35+
- Option) We can call the service while playing a rosbag and check the classification result.
36+
~~~~bash
37+
rosbag play data/s50_heavy/test_2018-04-26-22-22-53.bag
38+
rosservice call /object_state/left
39+
~~~~
40+
41+
## Creating a new estimator
42+
### Data collection
43+
- Repeat an object lifting task and rosbag topics. For example, we used a following launch file to record necessary topics from a Baxter robot.
44+
~~~~bash
45+
roslaunch object_estimator record_topic.launch
46+
~~~~
47+
Save data files in a data folder with label names. The folder name should include a label name as a part of it. See a following example that include "heavy" or "light".
48+
![Alt text](docs/data_folder.png?raw=true "Data folder")
49+
Each folder should contain a set of bag files recorded from the data collection process. The topic should include an end-effector wrench topic with message type, [EndpointState](msg/EndpointState.msg).
50+
51+
- Set proper rostopic and rosservice names to the launch file.
52+
53+
### Training and Testing
54+
Same as the above
55+
56+
57+
58+
## ETC
59+
### Arguments for the main launch file
60+
Under construction
61+
62+
### Option) Running procedure with the Baxter Experiment Stack
63+
- Running procedure for simulation
64+
~~~~bash
65+
roslaunch baxter_gazebo baxter_world.launch gui:=false
66+
rosrun rviz rviz
67+
rosrun baxter_tools enable_robot.py -e && rosrun baxter_tools tuck_arms.py -u
68+
roslaunch baxter_pipeline_daehyung.launch
69+
roslaunch manipulator_controller action_executive.launch
70+
rosrun manipulator_controller executive.py --r sim
71+
~~~~
72+
73+
- Test
74+
~~~~bash
75+
rostopic pub /speech_recognition std_msgs/String "data: 'pick up the left most'"
76+
~~~~

‎data/placeholder.txt

Whitespace-only changes.

‎docs/data_folder.png

42.6 KB
Loading

‎launch/full_cup_estimation.launch

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
<launch>
2+
3+
<!-- End-effector wrench topic with msg type: EndpointState -->
4+
<arg name="state_topic" default="/robot/limb/left/endpoint_state"/>
5+
6+
<!-- Estimation service name with type: None_String -->
7+
<arg name="srv_topic" default="object_state/left"/>
8+
9+
<!-- Raw data path in object_estimator pkg.-->
10+
<arg name="data_path" default="data"/>
11+
12+
<!-- Preprocessed file name-->
13+
<arg name="filename" default="empty_full_data.pkl"/>
14+
15+
<!-- input/output labels-->
16+
<arg name="input_labels" default="light_heavy"/>
17+
<arg name="output_labels" default="empty_full"/>
18+
19+
<!-- Enable preprocessing-->
20+
<arg name="en_preprocess" default="false"/>
21+
22+
<!-- Plot raw data-->
23+
<arg name="plot" default="false"/>
24+
25+
<!-- Enable Debug mode-->
26+
<arg name="debug_mode" default="false"/>
27+
28+
29+
<!-- Start the object state estimator server -->
30+
<node name="full_cup_est"
31+
pkg="object_estimator"
32+
type="obj_state_classifier.py"
33+
args="--state_topic $(arg state_topic)
34+
--srv_topic $(arg srv_topic)
35+
--dp $(arg data_path)
36+
--saved_filename $(arg filename)
37+
--preprocessing $(arg en_preprocess)
38+
--input_labels $(arg input_labels)
39+
--output_labels $(arg output_labels)
40+
--plot $(arg plot)
41+
--debug $(arg debug_mode)"
42+
required="true"
43+
output="screen" />
44+
45+
</launch>

‎launch/record_topic.launch

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<launch>
2+
3+
<arg name="fname" default="test"/>
4+
5+
<node pkg="topic_tools" type="throttle" name="tf_throttle"
6+
args="messages /tf 50" />
7+
8+
<node pkg="rosbag" type="record" name="rosbag" args="record -o
9+
$(find object_estimator)/data/$(arg fname)
10+
/tf_throttle
11+
/robot/limb/left/endpoint_state
12+
" output="screen"/>
13+
</launch>

‎msg/EndpointState.msg

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
Header header
2+
geometry_msgs/Pose pose
3+
geometry_msgs/Twist twist
4+
geometry_msgs/Wrench wrench

‎package.xml

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
<?xml version="1.0"?>
2+
<package>
3+
<name>object_estimator</name>
4+
<version>0.0.1</version>
5+
<description>
6+
object_estimator
7+
</description>
8+
9+
<maintainer email="[email protected]">Daehyung Park</maintainer>
10+
<license>BSD</license>
11+
12+
<!-- Dependencies which this package needs to build itself. -->
13+
<buildtool_depend>catkin</buildtool_depend>
14+
15+
<build_depend>rospy</build_depend>
16+
<build_depend>geometry_msgs</build_depend>
17+
<build_depend>message_generation</build_depend>
18+
<build_depend>std_msgs</build_depend>
19+
20+
<run_depend>rospy</run_depend>
21+
<run_depend>geometry_msgs</run_depend>
22+
<run_depend>message_runtime</run_depend>
23+
<run_depend>std_msgs</run_depend>
24+
25+
</package>

‎params/placeholder.txt

Whitespace-only changes.

‎requirements.txt

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
numpy~=1.14.5
2+
matplotlib~=2.2.2
3+
sklearn~=0.19.1
4+
scipy~=1.1.0

‎setup.py

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
#!/usr/bin/env python
2+
from distutils.core import setup
3+
from catkin_pkg.python_setup import generate_distutils_setup
4+
5+
d = generate_distutils_setup()
6+
d['name'] = 'object_estimator',
7+
d['version'] = '0.0.1'
8+
d['description'] = 'Object internal state estimator using sensory signals'
9+
d['author'] = 'Daehyung Park'
10+
d['packages'] = ['object_estimator']
11+
d['package_dir'] = {'': 'src'}
12+
13+
setup(**d)

‎src/object_estimator/__init__.py

Whitespace-only changes.

‎src/object_estimator/data_collector.py

+100
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
#!/usr/bin/env python
2+
"""
3+
MIT License
4+
5+
Copyright (c) 2018 Daehyung Park
6+
7+
Permission is hereby granted, free of charge, to any person obtaining a copy
8+
of this software and associated documentation files (the "Software"), to deal
9+
in the Software without restriction, including without limitation the rights
10+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
11+
copies of the Software, and to permit persons to whom the Software is
12+
furnished to do so, subject to the following conditions:
13+
14+
The above copyright notice and this permission notice shall be included in all
15+
copies or substantial portions of the Software.
16+
17+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
18+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
19+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
20+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
21+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
22+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
23+
SOFTWARE.
24+
"""
25+
import rospy
26+
from std_msgs.msg import String
27+
import json
28+
import datetime
29+
import os, sys
30+
31+
import roslaunch
32+
import rospkg
33+
34+
35+
if __name__ == '__main__':
36+
"""This is a sample code for repetitive data collection
37+
"""
38+
rospy.init_node('data_collector')
39+
40+
rospack = rospkg.RosPack()
41+
event_running= False
42+
cur_event = None
43+
44+
def seq_finished_callback(msg):
45+
global event_running
46+
event_running = False
47+
48+
def event_callback(msg):
49+
global cur_event
50+
cur_event=msg.data
51+
52+
rospy.sleep(1.0)
53+
54+
# Communication
55+
pub = rospy.Publisher('/symbol_grounding', String, queue_size=10)
56+
rospy.Subscriber('sequence_finished', String, seq_finished_callback)
57+
rospy.Subscriber('current_event', String, event_callback)
58+
rospy.sleep(1.0)
59+
60+
path = os.path.join(rospack.get_path('object_estimator'),'launch/record_topic.launch')
61+
recording = False
62+
63+
64+
## while not rospy.is_shutdown():
65+
66+
## cmd = input("Command? ")
67+
68+
## if event_running:
69+
## rospy.sleep(1.0)
70+
## continue
71+
72+
# Publish a manipulation command
73+
d = {"timestamp": str(datetime.datetime.now()),
74+
"type": "assemble",
75+
"params": {"1": {"primitive_action": "record",
76+
"object": "cup1",
77+
"source": "cup1",
78+
"destination": "place-table2"}},
79+
"param_num": 1 }
80+
event_running = True
81+
pub.publish(json.dumps(d))
82+
83+
84+
# Run
85+
while not rospy.is_shutdown():
86+
if cur_event is None: continue
87+
if cur_event.find("z_approach")>=0 and recording is False:
88+
recording = True
89+
print "Start to record data"
90+
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
91+
roslaunch.configure_logging(uuid)
92+
launch = roslaunch.parent.ROSLaunchParent(uuid, [path])
93+
launch.start()
94+
elif cur_event.find("wing")>=0 and recording is True:
95+
recording = False
96+
print "Finish to record data"
97+
launch.shutdown()
98+
break
99+
rospy.sleep(0.1)
100+

‎src/object_estimator/hmm/__init__.py

Whitespace-only changes.

0 commit comments

Comments
 (0)