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

started working on node for human rviz window

Browse Source
This commit is contained in:
HRS_D
2019-02-04 19:22:19 +01:00
parent 6f124df8c7
commit 193dcff7a8
2 changed files with 395 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -40,5 +40,7 @@ catkin_install_python(PROGRAMS
add_executable(aruco_detector src/aruco_detector.cpp) add_executable(aruco_detector src/aruco_detector.cpp)
add_executable(speech src/speech.cpp) add_executable(speech src/speech.cpp)
add_executable(rviz_human src/rviz_human.cpp)
target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB}) target_link_libraries(aruco_detector ${catkin_LIBRARIES} ${aruco_LIB})
target_link_libraries(speech ${catkin_LIBRARIES} ${aruco_LIB}) target_link_libraries(speech ${catkin_LIBRARIES} ${aruco_LIB})
target_link_libraries(rviz_human ${catkin_LIBRARIES} ${aruco_LIB})

393
src/rviz_human.cpp Normal file
View File

@@ -0,0 +1,393 @@
/*
* Copyright (c) 2010, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <iostream>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.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", 10);
tf::TransformListener listener;
//uint32_t shape = visualization_msgs::Marker::CUBE;
//uint32_t shape2 = visualization_msgs::Marker::SPHERE;
int i = 0;
int walk = 0;
float l_leg_y = 0.0;
float r_leg_y = 0.0;
// suppose we already have the position of the center marker in the correct frame
float x_start = 2;
float y_start = 0;
float z_start = 1.3;
float x_0 = 2;
float y_0 = 0;
float z_0 = 1.3;
while (ros::ok())
{
/* // tried to subscribe to tf to recieve marker coordinates
tf::StampedTransform transform;
try{
listener.lookupTransform("/torso", "/aruco_0_frame",ros::Time(0), transform);
ROS_INFO("0: %f",transform.getOrigin()[0]);
ROS_INFO("1: %f",transform.getOrigin()[1]);
ROS_INFO("2: %f",transform.getOrigin()[2]);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
*/
if(i == 300)
{
x_0 = 5;
}
if(i == 600)
{
y_0 = 3;
}
if(i == 900)
{
z_0 = 0.3;
}
if(i == 1200)
{
x_0 = 2;
y_0 = 0;
z_0 = 1.3;
}
x_0 = -i*0.001+x_start;
visualization_msgs::Marker body;
visualization_msgs::Marker head;
visualization_msgs::Marker l_leg;
visualization_msgs::Marker r_leg;
visualization_msgs::Marker l_arm;
visualization_msgs::Marker r_arm;
visualization_msgs::Marker camera;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
body.header.frame_id = "/odom";
body.header.stamp = ros::Time::now();
head.header.frame_id = "/odom";
head.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();
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();
camera.header.frame_id = "/odom";
camera.header.stamp = ros::Time::now();
body.ns = "body";
body.id = 0;
head.ns = "head";
head.id = 1;
l_leg.ns = "l_leg";
l_leg.id = 2;
r_leg.ns = "r_leg";
r_leg.id = 3;
l_arm.ns = "l_arm";
l_arm.id = 4;
r_arm.ns = "r_arm";
r_arm.id = 5;
camera.ns = "r_arm";
camera.id = 6;
body.type = visualization_msgs::Marker::CUBE;
head.type = visualization_msgs::Marker::SPHERE;
l_leg.type = visualization_msgs::Marker::CYLINDER;
r_leg.type = visualization_msgs::Marker::CYLINDER;
l_arm.type = visualization_msgs::Marker::CYLINDER;
r_arm.type = visualization_msgs::Marker::CYLINDER;
camera.type = visualization_msgs::Marker::CUBE;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
body.action = visualization_msgs::Marker::ADD;
head.action = visualization_msgs::Marker::ADD;
l_arm.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
l_leg.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
camera.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
body.pose.position.x = x_0;
body.pose.position.y = y_0;
body.pose.position.z = z_0;
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 = x_0;
head.pose.position.y = y_0;
head.pose.position.z = z_0+0.5; //1.85
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_leg.pose.position.x = x_0;
l_leg.pose.position.y = y_0+0.2;
l_leg.pose.position.z = z_0-0.8; //0.4
l_leg.pose.orientation.x = 0.0;
l_leg.pose.orientation.y = l_leg_y;
l_leg.pose.orientation.z = 0.0;
l_leg.pose.orientation.w = 1.0;
r_leg.pose.position.x = x_0;
r_leg.pose.position.y = y_0-0.2;
r_leg.pose.position.z = z_0-0.8; //0.4
r_leg.pose.orientation.x = 0.0;
r_leg.pose.orientation.y = r_leg_y;
r_leg.pose.orientation.z = 0.0;
r_leg.pose.orientation.w = 1.0;
l_arm.pose.position.x = x_0;
l_arm.pose.position.y = y_0+0.4;
l_arm.pose.position.z = z_0+0.3; //1.6
l_arm.pose.orientation.x = 1.0;
l_arm.pose.orientation.y = 0.0;
l_arm.pose.orientation.z = 0.0;
l_arm.pose.orientation.w = 1.0;
r_arm.pose.position.x = x_0;
r_arm.pose.position.y = y_0-0.4;
r_arm.pose.position.z = z_0+0.3; //1.6
r_arm.pose.orientation.x = 1.0;
r_arm.pose.orientation.y = 0.0;
r_arm.pose.orientation.z = 0.0;
r_arm.pose.orientation.w = 1.0;
camera.pose.position.x = 0;
camera.pose.position.y = 0;
camera.pose.position.z = 0.5;
camera.pose.orientation.x = 0.0;
camera.pose.orientation.y = 0.0;
camera.pose.orientation.z = 0.0;
camera.pose.orientation.w = 0.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
body.scale.x = 0.2;
body.scale.y = 0.4;
body.scale.z = 0.8;
head.scale.x = 0.3;
head.scale.y = 0.3;
head.scale.z = 0.4;
l_leg.scale.x = 0.2;
l_leg.scale.y = 0.2;
l_leg.scale.z = 0.9;
r_leg.scale.x = 0.2;
r_leg.scale.y = 0.2;
r_leg.scale.z = 0.9;
l_arm.scale.x = 0.2;
l_arm.scale.y = 0.2;
l_arm.scale.z = 0.5;
r_arm.scale.x = 0.2;
r_arm.scale.y = 0.2;
r_arm.scale.z = 0.5;
camera.scale.x = 0.5;
camera.scale.y = 0.5;
camera.scale.z = 1;
// Set the color -- be sure to set alpha to something non-zero!
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_leg.color.r = 1.0f;
l_leg.color.g = 0.5f;
l_leg.color.b = 0.0f;
l_leg.color.a = 1.0;
r_leg.color.r = 1.0f;
r_leg.color.g = 0.5f;
r_leg.color.b = 0.0f;
r_leg.color.a = 1.0;
l_arm.color.r = 1.0f;
l_arm.color.g = 0.5f;
l_arm.color.b = 1.0f;
l_arm.color.a = 1.0;
r_arm.color.r = 1.0f;
r_arm.color.g = 0.5f;
r_arm.color.b = 1.0f;
r_arm.color.a = 1.0;
camera.color.r = 1.0f;
camera.color.g = 1.0f;
camera.color.b = 1.0f;
camera.color.a = 1.0;
body.lifetime = ros::Duration();
head.lifetime = ros::Duration();
l_leg.lifetime = ros::Duration();
r_leg.lifetime = ros::Duration();
l_arm.lifetime = ros::Duration();
r_arm.lifetime = ros::Duration();
camera.lifetime = ros::Duration();
// Publish the marker
while (marker_pub.getNumSubscribers() < 1)
{
if (!ros::ok())
{
return 0;
}
ROS_WARN_ONCE("Please create a subscriber to the marker");
sleep(1);
}
/*
// little walking animation
i++;
if(i % 100 == 0)
{
if(walk == 0)
{
walk = 1;
l_leg_y = 0.2;
r_leg_y = -0.2;
}
else
{
l_leg_y = -0.2;
r_leg_y = 0.2;
walk = 0;
}
}
*/
// move hole human
i++;
// publish markers
body.action = visualization_msgs::Marker::ADD;
head.action = visualization_msgs::Marker::ADD;
l_arm.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
l_leg.action = visualization_msgs::Marker::ADD;
r_arm.action = visualization_msgs::Marker::ADD;
marker_pub.publish(body);
marker_pub.publish(head);
marker_pub.publish(l_leg);
marker_pub.publish(r_leg);
marker_pub.publish(l_arm);
marker_pub.publish(r_arm);
marker_pub.publish(camera);
i = i +1;
//ROS_INFO("%d",i);
r.sleep();
// Delete markers
body.action = visualization_msgs::Marker::DELETE;
head.action = visualization_msgs::Marker::DELETE;
l_arm.action = visualization_msgs::Marker::DELETE;
r_arm.action = visualization_msgs::Marker::DELETE;
l_leg.action = visualization_msgs::Marker::DELETE;
r_arm.action = visualization_msgs::Marker::DELETE;
marker_pub.publish(body);
marker_pub.publish(head);
marker_pub.publish(l_leg);
marker_pub.publish(r_leg);
marker_pub.publish(l_arm);
marker_pub.publish(r_arm);
}
}