diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp index 6dd64ea..8df3062 100644 --- a/src/rviz_human.cpp +++ b/src/rviz_human.cpp @@ -14,6 +14,11 @@ int main( int argc, char** argv ) ros::Publisher marker_pub = n.advertise( "visualization_marker", 15); + + YAML::Node config = YAML::LoadFile("/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml"); + + ROS_INFO("%lf", config["arm"].as()); + 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()); // Set human to starting postion - torso.x = -1; - torso.y = 0; - torso.z = 0; + std::vector center = config["cr"].as >(); + std::vector l_sh = config["lsh"].as >(); + std::vector r_sh = config["rsh"].as >(); - l_hand.x = -1; - l_hand.y = 0.35; - l_hand.z = -0.2; + double size = config["arm"].as()*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(); + 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(); + 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() - center[0])*0.33; + double base_out_r = config["fw"].as() - 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(); }