pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

documented the rviz

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-28 15:22:43 +01:00
parent 1a65bb6af7
commit 74f25b5f90
1 changed files with 44 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

92
src/rviz_human.cpp
View File

@@ -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 <iostream>
#include <math.h>
#include <ros/ros.h>
@@ -14,36 +28,26 @@ 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;
// 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<double>());
// Set human to starting postion
/* Set human-model to starting postion (independent of camera input) */
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;
/* approximate size of human as 2.2 times the arm lenth */
double size = config["arm"].as<double>()*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<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
/* 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);