#include #include #include #include #include #include int main( int argc, char** argv ) { ros::init(argc, argv, "rviz_human"); ros::NodeHandle n; ros::Rate r(100); 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; // !!! 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 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; torso.x = center[0]; torso.y = center[1]; torso.z = center[2]; l_hand.x = center[0]; l_hand.y = center[1]+config["arm"].as(); l_hand.z = center[2]+l_sh[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_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 tf::StampedTransform aruco_0_tf; tf::StampedTransform aruco_1_tf; tf::StampedTransform aruco_2_tf; try { aruco_0.lookupTransform("/odom", "/Aruco_0_frame", ros::Time(0), aruco_0_tf); ROS_INFO("0: %f",aruco_0_tf.getOrigin()[0]); ROS_INFO("1: %f",aruco_0_tf.getOrigin()[1]); ROS_INFO("2: %f",aruco_0_tf.getOrigin()[2]); aruco_1.lookupTransform("/odom", "/Aruco_1_frame", ros::Time(0), aruco_1_tf); ROS_INFO("0_1: %f",aruco_1_tf.getOrigin()[0]); ROS_INFO("1_1: %f",aruco_1_tf.getOrigin()[1]); ROS_INFO("2_1: %f",aruco_1_tf.getOrigin()[2]); aruco_2.lookupTransform("/odom", "/Aruco_2_frame", ros::Time(0), aruco_2_tf); ROS_INFO("0_2: %f",aruco_2_tf.getOrigin()[0]); 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 torso.x = aruco_0_tf.getOrigin()[0]; torso.y = aruco_0_tf.getOrigin()[1]; torso.z = aruco_0_tf.getOrigin()[2]; l_hand.x = aruco_1_tf.getOrigin()[0]; l_hand.y = aruco_1_tf.getOrigin()[1]; l_hand.z = aruco_1_tf.getOrigin()[2]; r_hand.x = aruco_2_tf.getOrigin()[0]; r_hand.y = aruco_2_tf.getOrigin()[1]; r_hand.z = aruco_2_tf.getOrigin()[2]; } catch (tf::TransformException ex){ ROS_INFO("%s",ex.what()); ros::Duration(1.0).sleep(); } visualization_msgs::Marker body; visualization_msgs::Marker head; visualization_msgs::Marker l_shoulder; visualization_msgs::Marker r_shoulder; visualization_msgs::Marker l_joints; visualization_msgs::Marker r_joints; visualization_msgs::Marker l_arm; visualization_msgs::Marker r_arm; visualization_msgs::Marker l_legjoints; visualization_msgs::Marker r_legjoints; visualization_msgs::Marker l_leg; visualization_msgs::Marker r_leg; visualization_msgs::Marker camera; visualization_msgs::Marker base_in; visualization_msgs::Marker base_out; // Set the frame ID and timestamp body.header.frame_id = "/odom"; body.header.stamp = ros::Time::now(); head.header.frame_id = "/odom"; head.header.stamp = ros::Time::now(); l_shoulder.header.frame_id = "/odom"; l_shoulder.header.stamp = ros::Time::now(); r_shoulder.header.frame_id = "/odom"; r_shoulder.header.stamp = ros::Time::now(); l_joints.header.frame_id = "/odom"; l_joints.header.stamp = ros::Time::now(); r_joints.header.frame_id = "/odom"; r_joints.header.stamp = ros::Time::now(); l_arm.header.frame_id = "/odom"; l_arm.header.stamp = ros::Time::now(); r_arm.header.frame_id = "/odom"; r_arm.header.stamp = ros::Time::now(); l_legjoints.header.frame_id = "/odom"; l_legjoints.header.stamp = ros::Time::now(); r_legjoints.header.frame_id = "/odom"; r_legjoints.header.stamp = ros::Time::now(); l_leg.header.frame_id = "/odom"; l_leg.header.stamp = ros::Time::now(); r_leg.header.frame_id = "/odom"; r_leg.header.stamp = ros::Time::now(); camera.header.frame_id = "/odom"; camera.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 body.ns = "body"; body.id = 0; head.ns = "head"; head.id = 1; l_shoulder.ns = "l_shoulder"; l_shoulder.id = 2; r_shoulder.ns = "r_shoulder"; r_shoulder.id = 3; l_joints.ns = "l_joints"; l_joints.id = 4; r_joints.ns = "l_joints"; r_joints.id = 5; l_arm.ns = "l_arm"; l_arm.id = 6; r_arm.ns = "r_arm"; r_arm.id = 7; l_legjoints.ns = "l_legjoints"; l_legjoints.id = 8; r_legjoints.ns = "l_legjoints"; r_legjoints.id = 9; l_leg.ns = "l_leg"; l_leg.id = 10; r_leg.ns = "r_leg"; r_leg.id = 11; camera.ns = "camera"; camera.id = 12; base_in.ns = "base_in"; base_in.id = 13; base_out.ns = "base_out"; base_out.id = 14; // set marker shape body.type = visualization_msgs::Marker::CUBE; head.type = visualization_msgs::Marker::SPHERE; l_shoulder.type = visualization_msgs::Marker::CUBE; r_shoulder.type = visualization_msgs::Marker::CUBE; l_joints.type = visualization_msgs::Marker::POINTS; r_joints.type = visualization_msgs::Marker::POINTS; l_arm.type = visualization_msgs::Marker::LINE_STRIP; r_arm.type = visualization_msgs::Marker::LINE_STRIP; l_legjoints.type = visualization_msgs::Marker::POINTS; r_legjoints.type = visualization_msgs::Marker::POINTS; l_leg.type = visualization_msgs::Marker::LINE_STRIP; r_leg.type = visualization_msgs::Marker::LINE_STRIP; camera.type = visualization_msgs::Marker::SPHERE; base_in.type = visualization_msgs::Marker::SPHERE; base_out.type = visualization_msgs::Marker::SPHERE; // Set the marker action body.action = visualization_msgs::Marker::ADD; head.action = visualization_msgs::Marker::ADD; l_shoulder.action = visualization_msgs::Marker::ADD; r_shoulder.action = visualization_msgs::Marker::ADD; l_joints.action = visualization_msgs::Marker::ADD; r_joints.action = visualization_msgs::Marker::ADD; l_arm.action = visualization_msgs::Marker::ADD; r_arm.action = visualization_msgs::Marker::ADD; l_legjoints.action = visualization_msgs::Marker::ADD; r_legjoints.action = visualization_msgs::Marker::ADD; l_leg.action = visualization_msgs::Marker::ADD; r_leg.action = visualization_msgs::Marker::ADD; camera.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-0.05*size; body.pose.orientation.x = 0.0; body.pose.orientation.y = 0.0; body.pose.orientation.z = 0.0; body.pose.orientation.w = 1.0; head.pose.position.x = torso.x; head.pose.position.y = torso.y; 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+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+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; r_shoulder.pose.orientation.w = 1.0; l_joints.pose.orientation.w = 1.0; r_joints.pose.orientation.w = 1.0; l_arm.pose.orientation.w = 1.0; r_arm.pose.orientation.w = 1.0; l_legjoints.pose.orientation.w = 1.0; r_legjoints.pose.orientation.w = 1.0; l_leg.pose.orientation.w = 1.0; r_leg.pose.orientation.w = 1.0; camera.pose.position.x = 0; camera.pose.position.y = 0; camera.pose.position.z = 0.0; camera.pose.orientation.x = 0.0; camera.pose.orientation.y = 0.0; camera.pose.orientation.z = 0.0; camera.pose.orientation.w = 0.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.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; l_shoulder.scale.x = 0.125; l_shoulder.scale.y = 0.125; l_shoulder.scale.z = 0.125; r_shoulder.scale.x = 0.125; r_shoulder.scale.y = 0.125; r_shoulder.scale.z = 0.125; l_joints.scale.x = 0.1; l_joints.scale.y = 0.1; r_joints.scale.x = 0.1; r_joints.scale.y = 0.1; l_arm.scale.x = 0.1; l_arm.scale.y = 0.1; r_arm.scale.x = 0.1; r_arm.scale.y = 0.1; l_legjoints.scale.x = 0.1; l_legjoints.scale.y = 0.1; r_legjoints.scale.x = 0.1; r_legjoints.scale.y = 0.1; l_leg.scale.x = 0.1; l_leg.scale.y = 0.1; r_leg.scale.x = 0.1; r_leg.scale.y = 0.1; camera.scale.x = 0.2; camera.scale.y = 0.2; camera.scale.z = 0.2; base_in.scale.x = 2*base_in_r_x; base_in.scale.y = 2*base_in_r_y; base_in.scale.z = 0.1; base_out.scale.x = 2*base_out_r_x; base_out.scale.y = 2*base_out_r_y; base_out.scale.z = 0.01; // Set the color body.color.r = 0.0f; body.color.g = 1.0f; body.color.b = 0.5f; body.color.a = 1.0; head.color.r = 0.0f; head.color.g = 1.0f; head.color.b = 0.0f; head.color.a = 1.0; l_shoulder.color.r = 0.5f; l_shoulder.color.g = 0.2f; l_shoulder.color.b = 1.0f; l_shoulder.color.a = 1.0; r_shoulder.color.r = 0.5f; r_shoulder.color.g = 0.2f; r_shoulder.color.b = 1.0f; r_shoulder.color.a = 1.0; l_joints.color.r = 0.5f; l_joints.color.g = 0.2f; l_joints.color.b = 1.0f; l_joints.color.a = 1.0; r_joints.color.r = 0.5f; r_joints.color.g = 0.2f; r_joints.color.b = 1.0f; r_joints.color.a = 1.0; l_arm.color.r = 0.5f; l_arm.color.g = 0.2f; l_arm.color.b = 1.0f; l_arm.color.a = 1.0; r_arm.color.r = 0.5f; r_arm.color.g = 0.2f; r_arm.color.b = 1.0f; r_arm.color.a = 1.0; l_legjoints.color.r = 0.5f; l_legjoints.color.g = 0.2f; l_legjoints.color.b = 1.0f; l_legjoints.color.a = 1.0; r_legjoints.color.r = 0.5f; r_legjoints.color.g = 0.2f; r_legjoints.color.b = 1.0f; r_legjoints.color.a = 1.0; l_leg.color.r = 0.5f; l_leg.color.g = 0.2f; l_leg.color.b = 1.0f; l_leg.color.a = 1.0; r_leg.color.r = 0.5f; r_leg.color.g = 0.2f; r_leg.color.b = 1.0f; r_leg.color.a = 1.0; camera.color.r = 0.5f; camera.color.g = 0.5f; camera.color.b = 0.5f; camera.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 body.lifetime = ros::Duration(); head.lifetime = ros::Duration(); l_shoulder.lifetime = ros::Duration(); r_shoulder.lifetime = ros::Duration(); l_joints.lifetime = ros::Duration(); r_joints.lifetime = ros::Duration(); l_arm.lifetime = ros::Duration(); r_arm.lifetime = ros::Duration(); l_legjoints.lifetime = ros::Duration(); r_legjoints.lifetime = ros::Duration(); l_leg.lifetime = ros::Duration(); r_leg.lifetime = ros::Duration(); camera.lifetime = ros::Duration(); base_in.lifetime = ros::Duration(); base_out.lifetime = ros::Duration(); // create lines // create line for arms geometry_msgs::Point l_shoulder_point; geometry_msgs::Point r_shoulder_point; l_shoulder_point.x = torso.x; 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+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); l_arm.points.push_back(l_shoulder_point); l_arm.points.push_back(l_hand); r_joints.points.push_back(r_shoulder_point); r_joints.points.push_back(r_hand); r_arm.points.push_back(r_shoulder_point); r_arm.points.push_back(r_hand); // create line for legs geometry_msgs::Point l_hip; geometry_msgs::Point r_hip; geometry_msgs::Point l_knee; geometry_msgs::Point r_knee; geometry_msgs::Point l_foot; geometry_msgs::Point r_foot; l_hip.x = torso.x; 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.1; r_hip.z = torso.z-0.25*size; l_knee.x = torso.x; 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.1; r_knee.z = torso.z-0.45*size; l_foot.x = torso.x; 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.1; r_foot.z = torso.z-0.70*size; // push to points and lines l_legjoints.points.push_back(l_hip); l_legjoints.points.push_back(l_knee); l_legjoints.points.push_back(l_foot); l_leg.points.push_back(l_hip); l_leg.points.push_back(l_knee); l_leg.points.push_back(l_foot); r_legjoints.points.push_back(r_hip); r_legjoints.points.push_back(r_knee); r_legjoints.points.push_back(r_foot); r_leg.points.push_back(r_hip); r_leg.points.push_back(r_knee); r_leg.points.push_back(r_foot); // Publish the markers while (marker_pub.getNumSubscribers() < 1) { if (!ros::ok()) { return 0; } ROS_WARN_ONCE("Please create a subscriber to the marker"); sleep(1); } marker_pub.publish(body); marker_pub.publish(head); marker_pub.publish(l_shoulder); marker_pub.publish(r_shoulder); marker_pub.publish(l_joints); marker_pub.publish(r_joints); marker_pub.publish(l_arm); marker_pub.publish(r_arm); marker_pub.publish(l_legjoints); marker_pub.publish(r_legjoints); marker_pub.publish(l_leg); marker_pub.publish(r_leg); marker_pub.publish(camera); marker_pub.publish(base_in); marker_pub.publish(base_out); r.sleep(); } }