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

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