00001 /****************************************************************************** 00002 Copyright (c) 2017, Alexander W. Winkler. All rights reserved. 00003 00004 Redistribution and use in source and binary forms, with or without 00005 modification, are permitted provided that the following conditions are met: 00006 00007 * Redistributions of source code must retain the above copyright notice, this 00008 list of conditions and the following disclaimer. 00009 00010 * Redistributions in binary form must reproduce the above copyright notice, 00011 this list of conditions and the following disclaimer in the documentation 00012 and/or other materials provided with the distribution. 00013 00014 * Neither the name of the copyright holder nor the names of its 00015 contributors may be used to endorse or promote products derived from 00016 this software without specific prior written permission. 00017 00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00028 ******************************************************************************/ 00029 00030 #include <ros/ros.h> 00031 00032 #include <xpp_msgs/RobotStateCartesian.h> 00033 #include <xpp_msgs/topic_names.h> 00034 00035 #include <xpp_states/convert.h> 00036 #include <xpp_states/robot_state_cartesian.h> 00037 00038 00039 using namespace xpp; 00040 00041 int main(int argc, char *argv[]) 00042 { 00043 ros::init(argc, argv, "monped_publisher_node"); 00044 00045 ros::NodeHandle n; 00046 ros::Publisher state_pub = n.advertise<xpp_msgs::RobotStateCartesian>(xpp_msgs::robot_state_desired, 1); 00047 ROS_INFO_STREAM("Waiting for Subscriber..."); 00048 while(ros::ok() && state_pub.getNumSubscribers() == 0) 00049 ros::Rate(100).sleep(); 00050 ROS_INFO_STREAM("Subscriber to initial state connected"); 00051 00052 00053 // visualize the state of a one-legged hopper 00054 RobotStateCartesian hopper(1); 00055 00056 // publishes a sequence of states for a total duration of T spaced 0.01s apart. 00057 double T = 2.0; 00058 double t = 0.0; 00059 double dt = 0.01; 00060 while (t < T) 00061 { 00062 // base and foot follow half a sine motion up and down 00063 hopper.base_.lin.p_.z() = 0.7 - 0.05*sin(2*M_PI/(2*T)*t); 00064 hopper.ee_motion_.at(0).p_.z() = 0.1*sin(2*M_PI/(2*T)*t); 00065 hopper.ee_forces_.at(0).z() = 100; // N 00066 hopper.ee_contact_.at(0) = true; 00067 00068 state_pub.publish(Convert::ToRos(hopper)); 00069 00070 ros::spinOnce(); 00071 ros::Duration(dt).sleep(); // pause loop so visualization has correct speed. 00072 t += dt; 00073 } 00074 00075 00076 return 0; 00077 } 00078