pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

better teleoperation iface

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-09 16:47:30 +01:00
parent c1d51ce9d9
commit 90e24f2565
1 changed files with 139 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

235
src/rviz_human.cpp
View File

@@ -14,6 +14,11 @@ int main( int argc, char** argv )
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>( ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>(
"visualization_marker", 15); "visualization_marker", 15);
YAML::Node config = YAML::LoadFile("/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml");
ROS_INFO("%lf", config["arm"].as<double>());
tf::TransformListener aruco_0; tf::TransformListener aruco_0;
tf::TransformListener aruco_1; tf::TransformListener aruco_1;
tf::TransformListener aruco_2; tf::TransformListener aruco_2;
@@ -24,7 +29,9 @@ int main( int argc, char** argv )
geometry_msgs::Point torso; geometry_msgs::Point torso;
geometry_msgs::Point l_hand; geometry_msgs::Point l_hand;
geometry_msgs::Point r_hand; geometry_msgs::Point r_hand;
geometry_msgs::Point base_position; geometry_msgs::Point base_position_in;
geometry_msgs::Point base_position_out;
// !!! YAML PARSING WORKS LIKE THIS !!! // !!! YAML PARSING WORKS LIKE THIS !!!
// YAML::Node config = YAML::LoadFile( // YAML::Node config = YAML::LoadFile(
@@ -32,23 +39,36 @@ int main( int argc, char** argv )
// ROS_INFO("%lf", config["arm"].as<double>()); // ROS_INFO("%lf", config["arm"].as<double>());
// Set human to starting postion // Set human to starting postion
torso.x = -1; std::vector<double> center = config["cr"].as<std::vector<double> >();
torso.y = 0; std::vector<double> l_sh = config["lsh"].as<std::vector<double> >();
torso.z = 0; std::vector<double> r_sh = config["rsh"].as<std::vector<double> >();
l_hand.x = -1; double size = config["arm"].as<double>()*2;
l_hand.y = 0.35;
l_hand.z = -0.2;
r_hand.x = -1; torso.x = center[0];
r_hand.y = -0.35; torso.y = center[1];
r_hand.z = -0.2; torso.z = center[2];
l_hand.x = center[0];
l_hand.y = center[1]+config["arm"].as<double>();
l_hand.z = center[2]+l_sh[2];
r_hand.x = center[0];
r_hand.y = center[1]-config["arm"].as<double>();
r_hand.z = center[2]+r_sh[2];
base_position_in.x = center[0];
base_position_in.y = center[1];
base_position_in.z = torso.z-0.75*size;
base_position_out.x = center[0];
base_position_out.y = center[1];
base_position_out.z = torso.z-0.75*size;
double base_in_r = (config["fw"].as<double>() - center[0])*0.33;
double base_out_r = config["fw"].as<double>() - center[0];
base_position.x = -1;
base_position.y = 0;
base_position.z = -1;
double base_r = 2;
while (ros::ok()) while (ros::ok())
{ {
@@ -109,7 +129,8 @@ int main( int argc, char** argv )
visualization_msgs::Marker l_leg; visualization_msgs::Marker l_leg;
visualization_msgs::Marker r_leg; visualization_msgs::Marker r_leg;
visualization_msgs::Marker camera; visualization_msgs::Marker camera;
visualization_msgs::Marker base; visualization_msgs::Marker base_in;
visualization_msgs::Marker base_out;
// Set the frame ID and timestamp // Set the frame ID and timestamp
@@ -152,8 +173,12 @@ int main( int argc, char** argv )
camera.header.frame_id = "/odom"; camera.header.frame_id = "/odom";
camera.header.stamp = ros::Time::now(); camera.header.stamp = ros::Time::now();
base.header.frame_id = "/odom"; base_in.header.frame_id = "/odom";
base.header.stamp = ros::Time::now(); base_in.header.stamp = ros::Time::now();
base_out.header.frame_id = "/odom";
base_out.header.stamp = ros::Time::now();
// set namespace and id // set namespace and id
@@ -196,8 +221,11 @@ int main( int argc, char** argv )
camera.ns = "camera"; camera.ns = "camera";
camera.id = 12; camera.id = 12;
base.ns = "base"; base_in.ns = "base_in";
base.id = 13; base_in.id = 13;
base_out.ns = "base_out";
base_out.id = 14;
// set marker shape // set marker shape
@@ -214,7 +242,8 @@ int main( int argc, char** argv )
l_leg.type = visualization_msgs::Marker::LINE_STRIP; l_leg.type = visualization_msgs::Marker::LINE_STRIP;
r_leg.type = visualization_msgs::Marker::LINE_STRIP; r_leg.type = visualization_msgs::Marker::LINE_STRIP;
camera.type = visualization_msgs::Marker::SPHERE; camera.type = visualization_msgs::Marker::SPHERE;
base.type = visualization_msgs::Marker::SPHERE; base_in.type = visualization_msgs::Marker::SPHERE;
base_out.type = visualization_msgs::Marker::SPHERE;
// Set the marker action // Set the marker action
@@ -231,13 +260,14 @@ int main( int argc, char** argv )
l_leg.action = visualization_msgs::Marker::ADD; l_leg.action = visualization_msgs::Marker::ADD;
r_leg.action = visualization_msgs::Marker::ADD; r_leg.action = visualization_msgs::Marker::ADD;
camera.action = visualization_msgs::Marker::ADD; camera.action = visualization_msgs::Marker::ADD;
base.action = visualization_msgs::Marker::ADD; base_in.action = visualization_msgs::Marker::ADD;
base_out.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker // Set the pose of the marker
body.pose.position.x = torso.x; body.pose.position.x = torso.x;
body.pose.position.y = torso.y; body.pose.position.y = torso.y;
body.pose.position.z = torso.z; body.pose.position.z = torso.z-0.05*size;
body.pose.orientation.x = 0.0; body.pose.orientation.x = 0.0;
body.pose.orientation.y = 0.0; body.pose.orientation.y = 0.0;
body.pose.orientation.z = 0.0; body.pose.orientation.z = 0.0;
@@ -245,23 +275,23 @@ int main( int argc, char** argv )
head.pose.position.x = torso.x; head.pose.position.x = torso.x;
head.pose.position.y = torso.y; head.pose.position.y = torso.y;
head.pose.position.z = torso.z+0.5; head.pose.position.z = torso.z+0.25*size;
head.pose.orientation.x = 0.0; head.pose.orientation.x = 0.0;
head.pose.orientation.y = 0.0; head.pose.orientation.y = 0.0;
head.pose.orientation.z = 0.0; head.pose.orientation.z = 0.0;
head.pose.orientation.w = 1.0; head.pose.orientation.w = 1.0;
l_shoulder.pose.position.x = torso.x; l_shoulder.pose.position.x = torso.x;
l_shoulder.pose.position.y = torso.y+0.3; l_shoulder.pose.position.y = torso.y+l_sh[1]-0.05;
l_shoulder.pose.position.z = torso.z+0.3; l_shoulder.pose.position.z = torso.z+l_sh[2];
l_shoulder.pose.orientation.x = 0.0; l_shoulder.pose.orientation.x = 0.0;
l_shoulder.pose.orientation.y = 0.0; l_shoulder.pose.orientation.y = 0.0;
l_shoulder.pose.orientation.z = 0.0; l_shoulder.pose.orientation.z = 0.0;
l_shoulder.pose.orientation.w = 1.0; l_shoulder.pose.orientation.w = 1.0;
r_shoulder.pose.position.x = torso.x; r_shoulder.pose.position.x = torso.x;
r_shoulder.pose.position.y = torso.y-0.3; r_shoulder.pose.position.y = torso.y+r_sh[1]+0.05;
r_shoulder.pose.position.z = torso.z+0.3; r_shoulder.pose.position.z = torso.z+r_sh[2];
r_shoulder.pose.orientation.x = 0.0; r_shoulder.pose.orientation.x = 0.0;
r_shoulder.pose.orientation.y = 0.0; r_shoulder.pose.orientation.y = 0.0;
r_shoulder.pose.orientation.z = 0.0; r_shoulder.pose.orientation.z = 0.0;
@@ -285,45 +315,45 @@ int main( int argc, char** argv )
camera.pose.orientation.z = 0.0; camera.pose.orientation.z = 0.0;
camera.pose.orientation.w = 0.0; camera.pose.orientation.w = 0.0;
base.pose.position.x = base_position.x; base_in.pose.position.x = base_position_in.x;
base.pose.position.y = base_position.y; base_in.pose.position.y = base_position_in.y;
base.pose.position.z = base_position.z; base_in.pose.position.z = base_position_in.z;
base.pose.orientation.x = 0.0; base_in.pose.orientation.x = 0.0;
base.pose.orientation.y = 0.0; base_in.pose.orientation.y = 0.0;
base.pose.orientation.z = 0.0; base_in.pose.orientation.z = 0.0;
base.pose.orientation.w = 1.0; base_in.pose.orientation.w = 1.0;
base_out.pose.position.x = base_position_out.x;
base_out.pose.position.y = base_position_out.y;
base_out.pose.position.z = base_position_out.z;
base_out.pose.orientation.x = 0.0;
base_out.pose.orientation.y = 0.0;
base_out.pose.orientation.z = 0.0;
base_out.pose.orientation.w = 1.0;
// Set the scale of the marker (in meters) // Set the scale of the marker (in meters)
body.scale.x = 0.2; body.scale.x = 0.125*size;
body.scale.y = 0.4; body.scale.y = 0.3*size;
body.scale.z = 0.8; body.scale.z = 0.4*size;
head.scale.x = 0.3; head.scale.x = 0.125*size;
head.scale.y = 0.3; head.scale.y = 0.125*size;
head.scale.z = 0.4; head.scale.z = 0.15*size;
l_leg.scale.x = 0.2; l_shoulder.scale.x = 0.125;
l_leg.scale.y = 0.2; l_shoulder.scale.y = 0.125;
l_leg.scale.z = 0.9; l_shoulder.scale.z = 0.125;
r_leg.scale.x = 0.2; r_shoulder.scale.x = 0.125;
r_leg.scale.y = 0.2; r_shoulder.scale.y = 0.125;
r_leg.scale.z = 0.9; r_shoulder.scale.z = 0.125;
l_shoulder.scale.x = 0.2; l_joints.scale.x = 0.1;
l_shoulder.scale.y = 0.2; l_joints.scale.y = 0.1;
l_shoulder.scale.z = 0.2;
r_shoulder.scale.x = 0.2; r_joints.scale.x = 0.1;
r_shoulder.scale.y = 0.2; r_joints.scale.y = 0.1;
r_shoulder.scale.z = 0.2;
l_joints.scale.x = 0.2;
l_joints.scale.y = 0.2;
r_joints.scale.x = 0.2;
r_joints.scale.y = 0.2;
l_arm.scale.x = 0.1; l_arm.scale.x = 0.1;
l_arm.scale.y = 0.1; l_arm.scale.y = 0.1;
@@ -331,25 +361,29 @@ int main( int argc, char** argv )
r_arm.scale.x = 0.1; r_arm.scale.x = 0.1;
r_arm.scale.y = 0.1; r_arm.scale.y = 0.1;
l_legjoints.scale.x = 0.2; l_legjoints.scale.x = 0.1;
l_legjoints.scale.y = 0.2; l_legjoints.scale.y = 0.1;
r_legjoints.scale.x = 0.2; r_legjoints.scale.x = 0.1;
r_legjoints.scale.y = 0.2; r_legjoints.scale.y = 0.1;
l_leg.scale.x = 0.2; l_leg.scale.x = 0.1;
l_leg.scale.y = 0.2; l_leg.scale.y = 0.1;
r_leg.scale.x = 0.2; r_leg.scale.x = 0.1;
r_leg.scale.y = 0.2; r_leg.scale.y = 0.1;
camera.scale.x = 0.5; camera.scale.x = 0.2;
camera.scale.y = 0.5; camera.scale.y = 0.2;
camera.scale.z = 0.5; camera.scale.z = 0.2;
base.scale.x = base_r; base_in.scale.x = 2*base_in_r;
base.scale.y = base_r; base_in.scale.y = 2*base_in_r;
base.scale.z = 0.01; base_in.scale.z = 0.1;
base_out.scale.x = 2*base_out_r;
base_out.scale.y = 2*base_out_r;
base_out.scale.z = 0.01;
// Set the color // Set the color
@@ -413,15 +447,20 @@ int main( int argc, char** argv )
r_leg.color.b = 1.0f; r_leg.color.b = 1.0f;
r_leg.color.a = 1.0; r_leg.color.a = 1.0;
camera.color.r = 1.0f; camera.color.r = 0.5f;
camera.color.g = 1.0f; camera.color.g = 0.5f;
camera.color.b = 1.0f; camera.color.b = 0.5f;
camera.color.a = 1.0; camera.color.a = 1.0;
base.color.r = 1.0f; base_in.color.r = 1.0f;
base.color.g = 0.0f; base_in.color.g = 0.0f;
base.color.b = 0.0f; base_in.color.b = 0.0f;
base.color.a = 1.0; base_in.color.a = 1.0;
base_out.color.r = 0.0f;
base_out.color.g = 1.0f;
base_out.color.b = 0.5f;
base_out.color.a = 1.0;
// set lifetime // set lifetime
@@ -438,7 +477,8 @@ int main( int argc, char** argv )
l_leg.lifetime = ros::Duration(); l_leg.lifetime = ros::Duration();
r_leg.lifetime = ros::Duration(); r_leg.lifetime = ros::Duration();
camera.lifetime = ros::Duration(); camera.lifetime = ros::Duration();
base.lifetime = ros::Duration(); base_in.lifetime = ros::Duration();
base_out.lifetime = ros::Duration();
// create lines // create lines
// create line for arms // create line for arms
@@ -447,12 +487,12 @@ int main( int argc, char** argv )
geometry_msgs::Point r_shoulder_point; geometry_msgs::Point r_shoulder_point;
l_shoulder_point.x = torso.x; l_shoulder_point.x = torso.x;
l_shoulder_point.y = torso.y+0.3; l_shoulder_point.y = torso.y+l_sh[1];
l_shoulder_point.z = torso.z+0.3; l_shoulder_point.z = torso.z+l_sh[2];
r_shoulder_point.x = torso.x; r_shoulder_point.x = torso.x;
r_shoulder_point.y = torso.y-0.3; r_shoulder_point.y = torso.y+r_sh[1];
r_shoulder_point.z = torso.z+0.3; r_shoulder_point.z = torso.z+r_sh[2];
l_joints.points.push_back(l_shoulder_point); l_joints.points.push_back(l_shoulder_point);
l_joints.points.push_back(l_hand); l_joints.points.push_back(l_hand);
@@ -474,28 +514,28 @@ int main( int argc, char** argv )
geometry_msgs::Point r_foot; geometry_msgs::Point r_foot;
l_hip.x = torso.x; l_hip.x = torso.x;
l_hip.y = torso.y+0.2; l_hip.y = torso.y+0.1;
l_hip.z = torso.z-0.15; l_hip.z = torso.z-0.25*size;
r_hip.x = torso.x; r_hip.x = torso.x;
r_hip.y = torso.y-0.2; r_hip.y = torso.y-0.1;
r_hip.z = torso.z-0.15; r_hip.z = torso.z-0.25*size;
l_knee.x = torso.x; l_knee.x = torso.x;
l_knee.y = torso.y+0.2; l_knee.y = torso.y+0.1;
l_knee.z = torso.z-0.55; l_knee.z = torso.z-0.45*size;
r_knee.x = torso.x; r_knee.x = torso.x;
r_knee.y = torso.y-0.2; r_knee.y = torso.y-0.1;
r_knee.z = torso.z-0.55; r_knee.z = torso.z-0.45*size;
l_foot.x = torso.x; l_foot.x = torso.x;
l_foot.y = torso.y+0.2; l_foot.y = torso.y+0.1;
l_foot.z = torso.z-0.95; l_foot.z = torso.z-0.70*size;
r_foot.x = torso.x; r_foot.x = torso.x;
r_foot.y = torso.y-0.2; r_foot.y = torso.y-0.1;
r_foot.z = torso.z-0.95; r_foot.z = torso.z-0.70*size;
// push to points and lines // push to points and lines
@@ -541,7 +581,8 @@ int main( int argc, char** argv )
marker_pub.publish(l_leg); marker_pub.publish(l_leg);
marker_pub.publish(r_leg); marker_pub.publish(r_leg);
marker_pub.publish(camera); marker_pub.publish(camera);
marker_pub.publish(base); marker_pub.publish(base_in);
marker_pub.publish(base_out);
r.sleep(); r.sleep();
} }