pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
1a65bb6af795a3b832e2329e29af34e57038587a
teleoperation/src/rviz_human.cpp
Pavel Lutskov 1a65bb6af7 finalizing
2019-02-10 17:33:26 +01:00

593 lines
18 KiB
C++
Raw Blame History

#include <iostream>
#include <math.h>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <yaml-cpp/yaml.h>
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_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;
// 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<double>());
// Set human to starting postion
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> >();
double size = config["arm"].as<double>()*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<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_x = (config["fw"].as<double>() - center[0])*0.33;
double base_out_r_x = config["fw"].as<double>() - center[0];
double base_in_r_y = (config["rt"].as<double>() - center[1])*0.33;
double base_out_r_y = (config["rt"].as<double>() - 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();
}
}
Reference in New Issue View Git Blame Copy Permalink