From 74f25b5f909927d0773324171f9f41ffed316d7b Mon Sep 17 00:00:00 2001 From: Pavel Lutskov Date: Thu, 28 Feb 2019 15:22:43 +0100 Subject: [PATCH] documented the rviz --- src/rviz_human.cpp | 92 ++++++++++++++++++++++------------------------ 1 file changed, 44 insertions(+), 48 deletions(-) diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp index ace96c9..3d764cc 100644 --- a/src/rviz_human.cpp +++ b/src/rviz_human.cpp @@ -1,3 +1,17 @@ +/** + * Publishes model of human,movement zone and camera consisting of markers to rviz. + * + * A YAML config file is opended to get the starting postion of the model + * and to define the size of the movement zone. + * Tf is subscribed to get the positions of the aruco markers on the + * operator. Using those inputs a model is published to rivz in each + * iteration. + * + * default.yaml: file contains points of the operator and the size + * of the action zone which are determined in a calibration + * step. + */ + #include #include #include @@ -14,36 +28,26 @@ 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; - // suppose we already have the position of the center marker - // in the correct frame - geometry_msgs::Point torso; geometry_msgs::Point l_hand; geometry_msgs::Point r_hand; geometry_msgs::Point base_position_in; geometry_msgs::Point base_position_out; + + /* Open YAML config file for starting position */ + YAML::Node config = YAML::LoadFile("/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml"); - - // !!! YAML PARSING WORKS LIKE THIS !!! - // YAML::Node config = YAML::LoadFile( - // "/home/hrs_e/catkin_ws/src/teleoperation/config/default.yaml"); - // ROS_INFO("%lf", config["arm"].as()); - - // Set human to starting postion + /* Set human-model to starting postion (independent of camera input) */ std::vector center = config["cr"].as >(); std::vector l_sh = config["lsh"].as >(); std::vector r_sh = config["rsh"].as >(); - double size = config["arm"].as()*2; + /* approximate size of human as 2.2 times the arm lenth */ + double size = config["arm"].as()*2.2; torso.x = center[0]; torso.y = center[1]; @@ -65,18 +69,16 @@ int main( int argc, char** argv ) base_position_out.y = center[1]; base_position_out.z = torso.z-0.75*size; + /* define radius of inner and outer circle of movement zone */ double base_in_r_x = (config["fw"].as() - center[0])*0.33; double base_out_r_x = config["fw"].as() - center[0]; double base_in_r_y = (config["rt"].as() - center[1])*0.33; double base_out_r_y = (config["rt"].as() - center[1]); - - while (ros::ok()) { - - // tried to subscribe to tf to recieve marker coordinates + /* Subscribe to tf to recieve marker coordinates */ tf::StampedTransform aruco_0_tf; tf::StampedTransform aruco_1_tf; tf::StampedTransform aruco_2_tf; @@ -100,7 +102,7 @@ int main( int argc, char** argv ) ROS_INFO("1_2: %f",aruco_2_tf.getOrigin()[1]); ROS_INFO("2_2: %f",aruco_2_tf.getOrigin()[2]); - // update torso and hand positions + /* Update positions of torso and hands */ torso.x = aruco_0_tf.getOrigin()[0]; torso.y = aruco_0_tf.getOrigin()[1]; torso.z = aruco_0_tf.getOrigin()[2]; @@ -119,6 +121,7 @@ int main( int argc, char** argv ) ros::Duration(1.0).sleep(); } + /* Create markers */ visualization_msgs::Marker body; visualization_msgs::Marker head; visualization_msgs::Marker l_shoulder; @@ -135,8 +138,7 @@ int main( int argc, char** argv ) visualization_msgs::Marker base_in; visualization_msgs::Marker base_out; - // Set the frame ID and timestamp - + /* Set the frame ID and timestamp */ body.header.frame_id = "/odom"; body.header.stamp = ros::Time::now(); @@ -183,8 +185,7 @@ int main( int argc, char** argv ) base_out.header.stamp = ros::Time::now(); - // set namespace and id - + /* Set namespace and id */ body.ns = "body"; body.id = 0; @@ -230,8 +231,7 @@ int main( int argc, char** argv ) base_out.ns = "base_out"; base_out.id = 14; - // set marker shape - + /* Set marker shape */ body.type = visualization_msgs::Marker::CUBE; head.type = visualization_msgs::Marker::SPHERE; l_shoulder.type = visualization_msgs::Marker::CUBE; @@ -248,8 +248,7 @@ int main( int argc, char** argv ) base_in.type = visualization_msgs::Marker::SPHERE; base_out.type = visualization_msgs::Marker::SPHERE; - // Set the marker action - + /* Set the marker action */ body.action = visualization_msgs::Marker::ADD; head.action = visualization_msgs::Marker::ADD; l_shoulder.action = visualization_msgs::Marker::ADD; @@ -266,8 +265,7 @@ int main( int argc, char** argv ) 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.y = torso.y; body.pose.position.z = torso.z-0.05*size; @@ -334,15 +332,14 @@ int main( int argc, char** argv ) 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.125*size; body.scale.y = 0.3*size; body.scale.z = 0.4*size; - head.scale.x = 0.125*size; - head.scale.y = 0.125*size; - head.scale.z = 0.15*size; + head.scale.x = 0.14*size; + head.scale.y = 0.14*size; + head.scale.z = 0.16*size; l_shoulder.scale.x = 0.125; l_shoulder.scale.y = 0.125; @@ -388,8 +385,7 @@ int main( int argc, char** argv ) base_out.scale.y = 2*base_out_r_y; base_out.scale.z = 0.01; - // Set the color - + /* Set the color */ body.color.r = 0.0f; body.color.g = 1.0f; body.color.b = 0.5f; @@ -465,8 +461,7 @@ int main( int argc, char** argv ) base_out.color.b = 0.5f; base_out.color.a = 1.0; - // set lifetime - + /* Set lifetime */ body.lifetime = ros::Duration(); head.lifetime = ros::Duration(); l_shoulder.lifetime = ros::Duration(); @@ -483,8 +478,8 @@ int main( int argc, char** argv ) base_in.lifetime = ros::Duration(); base_out.lifetime = ros::Duration(); - // create lines - // create line for arms + /* Create lines for arms and legs */ + /* Lines for arms */ geometry_msgs::Point l_shoulder_point; geometry_msgs::Point r_shoulder_point; @@ -497,6 +492,7 @@ int main( int argc, char** argv ) r_shoulder_point.y = torso.y+r_sh[1]; r_shoulder_point.z = torso.z+r_sh[2]; + /* Push arm joint-points to lines */ l_joints.points.push_back(l_shoulder_point); l_joints.points.push_back(l_hand); l_arm.points.push_back(l_shoulder_point); @@ -507,7 +503,7 @@ int main( int argc, char** argv ) r_arm.points.push_back(r_shoulder_point); r_arm.points.push_back(r_hand); - // create line for legs + /* Lines for legs */ geometry_msgs::Point l_hip; geometry_msgs::Point r_hip; @@ -540,8 +536,7 @@ int main( int argc, char** argv ) r_foot.y = torso.y-0.1; r_foot.z = torso.z-0.70*size; - // push to points and lines - + /* Push leg joint-points to lines */ l_legjoints.points.push_back(l_hip); l_legjoints.points.push_back(l_knee); l_legjoints.points.push_back(l_foot); @@ -559,8 +554,7 @@ int main( int argc, char** argv ) r_leg.points.push_back(r_foot); - // Publish the markers - + /* Publish the markers */ while (marker_pub.getNumSubscribers() < 1) { if (!ros::ok()) @@ -570,7 +564,7 @@ int main( int argc, char** argv ) ROS_WARN_ONCE("Please create a subscriber to the marker"); sleep(1); } - + /* Human model markers */ marker_pub.publish(body); marker_pub.publish(head); marker_pub.publish(l_shoulder); @@ -583,7 +577,9 @@ int main( int argc, char** argv ) marker_pub.publish(r_legjoints); marker_pub.publish(l_leg); marker_pub.publish(r_leg); + /* Camera marker */ marker_pub.publish(camera); + /* Movement zone markers */ marker_pub.publish(base_in); marker_pub.publish(base_out);