Revision 89374d69
| examples/meka/src/main.cpp | ||
|---|---|---|
| 22 | 22 |
|
| 23 | 23 |
*/ |
| 24 | 24 |
|
| 25 |
if (argc != 4){
|
|
| 26 |
printf("> ERROR: invalid number of parameters passed to server!\n\nusage: %s <humotion base topic> <jointstates topic> <control topic> (example: %s /meka /joint_states /meka_roscontrol/head_position_trajectory_controller/command)\n\n",argv[0],argv[0]);
|
|
| 27 |
exit(EXIT_FAILURE); |
|
| 28 |
} |
|
| 29 |
|
|
| 25 | 30 |
//create humotion interface |
| 26 |
string scope = "/"; |
|
| 27 |
MekaJointInterface *jointinterface = new MekaJointInterface(scope); |
|
| 28 |
humotion::server::Server *humotion_server = new humotion::server::Server("/flobi", "ROS", jointinterface);
|
|
| 31 |
string humotion_scope = argv[1]; |
|
| 32 |
string jointstates_scope = argv[2]; |
|
| 33 |
string control_scope = argv[3]; |
|
| 34 |
|
|
| 35 |
|
|
| 36 |
MekaJointInterface *jointinterface = new MekaJointInterface(jointstates_scope, control_scope); |
|
| 37 |
humotion::server::Server *humotion_server = new humotion::server::Server(humotion_scope, "ROS", jointinterface); |
|
| 29 | 38 |
|
| 30 | 39 |
//finally run it |
| 31 | 40 |
jointinterface->run(); |
| examples/meka/src/mekajointinterface.cpp | ||
|---|---|---|
| 57 | 57 |
//! constructor |
| 58 | 58 |
MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
|
| 59 | 59 |
scope = _scope; |
| 60 |
string js_topic = scope + "joint_states";
|
|
| 61 |
string target_topic = scope + "meka_roscontrol/head_position_trajectory_controller/command"; |
|
| 60 |
string js_topic = scope + "/joint_states";
|
|
| 61 |
string target_topic = scope + "/meka_roscontrol/head_position_trajectory_controller/command";
|
|
| 62 | 62 |
|
| 63 | 63 |
//subscribe to meka joint states |
| 64 | 64 |
int argc = 0; |
| include/humotion/server/controller.h | ||
|---|---|---|
| 49 | 49 |
|
| 50 | 50 |
private: |
| 51 | 51 |
void add_motion_generator(MotionGenerator *m); |
| 52 |
GazeState relative_gaze_to_absolute_gaze(GazeState relative); |
|
| 52 | 53 |
|
| 53 | 54 |
typedef std::vector<MotionGenerator *> motion_generator_vector_t; |
| 54 | 55 |
motion_generator_vector_t motion_generator_vector; |
| src/client/middleware_ros.cpp | ||
|---|---|---|
| 131 | 131 |
msg.eyeblink_request_left = gaze_state.eyeblink_request_left; |
| 132 | 132 |
msg.eyeblink_request_right = gaze_state.eyeblink_request_right; |
| 133 | 133 |
|
| 134 |
msg.type = humotion::gaze::ABSOLUTE; |
|
| 134 |
if (gaze_state.type == GazeState::ABSOLUTE){
|
|
| 135 |
msg.type = humotion::gaze::ABSOLUTE; |
|
| 136 |
}else{
|
|
| 137 |
msg.type = humotion::gaze::RELATIVE; |
|
| 138 |
} |
|
| 135 | 139 |
|
| 136 | 140 |
//add position to send queue |
| 137 | 141 |
gaze_target_publisher.publish(msg); |
| src/client/middleware_rsb.cpp | ||
|---|---|---|
| 110 | 110 |
} |
| 111 | 111 |
|
| 112 | 112 |
//! send mouth target to server |
| 113 |
void MiddlewareRSB::send_gaze_target(){
|
|
| 113 |
void MiddlewareRSB::send_gaze_target(int gaze_type){
|
|
| 114 | 114 |
//build target packet: |
| 115 | 115 |
boost::shared_ptr<rst::robot::HumotionGazeTarget> request(new rst::robot::HumotionGazeTarget()); |
| 116 | 116 |
|
| ... | ... | |
| 132 | 132 |
request->set_eyeblink_request_left(gaze_state.eyeblink_request_left); |
| 133 | 133 |
request->set_eyeblink_request_right(gaze_state.eyeblink_request_right); |
| 134 | 134 |
|
| 135 |
request->set_type(rst::robot::HumotionGazeTarget::ABSOLUTE); |
|
| 135 |
if (gaze_state.type == GazeState::ABSOLUTE){
|
|
| 136 |
request->set_type(rst::robot::HumotionGazeTarget::ABSOLUTE); |
|
| 137 |
}else{
|
|
| 138 |
request->set_type(rst::robot::HumotionGazeTarget::RELATIVE); |
|
| 139 |
} |
|
| 140 |
|
|
| 136 | 141 |
|
| 137 | 142 |
//add position to send queue |
| 138 | 143 |
gaze_target_informer->publish(request); |
| src/server/controller.cpp | ||
|---|---|---|
| 86 | 86 |
} |
| 87 | 87 |
} |
| 88 | 88 |
|
| 89 |
GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){
|
|
| 90 |
GazeState absolute_gaze = relative; |
|
| 91 |
|
|
| 92 |
//incoming gaze state wants to set a relative gaze angle |
|
| 93 |
//in order to calc the new absolute gaze, we need to go back |
|
| 94 |
//in time and find out where the head was pointing at that specific time: |
|
| 95 |
double relative_target_timestamp = relative.timestamp; |
|
| 96 |
|
|
| 97 |
//fetch head / camera pose during that timestamp: |
|
| 98 |
double neck_pan = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_interpolated_value(relative_target_timestamp); |
|
| 99 |
double eye_l_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_LEFT_LR).get_interpolated_value(relative_target_timestamp); |
|
| 100 |
double eye_r_pan = joint_interface->get_ts_position(JointInterface::ID_EYES_RIGHT_LR).get_interpolated_value(relative_target_timestamp); |
|
| 101 |
double pan = neck_pan + (eye_l_pan + eye_r_pan)/2.0; |
|
| 102 |
// |
|
| 103 |
double neck_tilt = joint_interface->get_ts_position(JointInterface::ID_NECK_TILT).get_interpolated_value(relative_target_timestamp); |
|
| 104 |
double eye_tilt = joint_interface->get_ts_position(JointInterface::ID_EYES_BOTH_UD).get_interpolated_value(relative_target_timestamp); |
|
| 105 |
double tilt = neck_tilt + eye_tilt; |
|
| 106 |
// |
|
| 107 |
double roll = joint_interface->get_ts_position(JointInterface::ID_NECK_ROLL).get_interpolated_value(relative_target_timestamp); |
|
| 108 |
|
|
| 109 |
//build up absolute target: |
|
| 110 |
absolute_gaze.type = GazeState::ABSOLUTE; |
|
| 111 |
absolute_gaze.pan = pan + relative.pan; |
|
| 112 |
absolute_gaze.tilt = tilt + relative.tilt; |
|
| 113 |
absolute_gaze.roll = roll + relative.roll; |
|
| 114 |
|
|
| 115 |
//FIXME: use ros TF for that calculation... |
|
| 116 |
//see http://wiki.ros.org/tf/Tutorials/Time%20travel%20with%20tf%20%28C%2B%2B%29 |
|
| 117 |
//ros::Time past = now - ros::Duration(5.0); |
|
| 118 |
//listener.waitForTransform("/turtle2", now,J "/turtle1", past, "/world", ros::Duration(1.0));
|
|
| 119 |
//listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);
|
|
| 120 |
|
|
| 121 |
return absolute_gaze; |
|
| 122 |
} |
|
| 123 |
|
|
| 124 |
|
|
| 89 | 125 |
|
| 90 | 126 |
//! update gaze target: |
| 91 | 127 |
//! \param GazeState with target values for the overall gaze |
| 92 | 128 |
void Controller::set_gaze_target(GazeState new_gaze_target){
|
| 129 |
GazeState target_gaze; |
|
| 130 |
|
|
| 131 |
//relative or absolute gaze update? |
|
| 132 |
if (new_gaze_target.type == GazeState::RELATIVE){
|
|
| 133 |
//relative gaze target -> calculate target angles |
|
| 134 |
target_gaze = relative_gaze_to_absolute_gaze(new_gaze_target); |
|
| 135 |
}else{
|
|
| 136 |
//already absolute gaze, set this |
|
| 137 |
target_gaze = new_gaze_target; |
|
| 138 |
} |
|
| 139 |
|
|
| 140 |
|
|
| 93 | 141 |
for(motion_generator_vector_t::iterator it = motion_generator_vector.begin(); it<motion_generator_vector.end(); it++){
|
| 94 |
(*it)->set_gaze_target(new_gaze_target);
|
|
| 142 |
(*it)->set_gaze_target(target_gaze);
|
|
| 95 | 143 |
} |
| 96 | 144 |
} |
| 97 | 145 |
|
Also available in: Unified diff