diff --git a/CMakeLists.txt b/CMakeLists.txt index d3c2f33..073ae6f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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}) diff --git a/src/rviz_human.cpp b/src/rviz_human.cpp new file mode 100644 index 0000000..550c870 --- /dev/null +++ b/src/rviz_human.cpp @@ -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 +#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", 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); + + + } + +} + +