started working on node for human rviz window
This commit is contained in:
HRS_D
@@ -40,5 +40,7 @@ catkin_install_python(PROGRAMS
|
||||
|
||||
add_executable(aruco_detector src/aruco_detector.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(speech ${catkin_LIBRARIES} ${aruco_LIB})
|
||||
target_link_libraries(rviz_human ${catkin_LIBRARIES} ${aruco_LIB})
|
||||
|
||||
393
src/rviz_human.cpp
Normal file
393
src/rviz_human.cpp
Normal 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);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user