test_odom.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <pr2_mechanism_controllers/Pose3D.h>
36 #include <ros/node.h>
37 #include <geometry_msgs/Twist.h>
38 #include <nav_msgs/Odometry.h>
39 #include <geometry_msgs/Quaternion.h>
40 #include <iostream>
41 #include <fstream>
42 
43 static int done = 0;
44 
45 void finalize(int donecare)
46 {
47  done = 1;
48 }
49 
50 
52 // Return the rotation in Euler angles
53 libTF::Vector GetAsEuler(geometry_msgs::Quaternion quat)
54 {
55  libTF::Vector vec;
56 
57  double squ;
58  double sqx;
59  double sqy;
60  double sqz;
61 
62 // this->Normalize();
63 
64  squ = quat.w * quat.w;
65  sqx = quat.x * quat.x;
66  sqy = quat.y * quat.y;
67  sqz = quat.z * quat.z;
68 
69  // Roll
70  vec.x = atan2(2 * (quat.y*quat.z + quat.w*quat.x), squ - sqx - sqy + sqz);
71 
72  // Pitch
73  vec.y = asin(-2 * (quat.x*quat.z - quat.w * quat.y));
74 
75  // Yaw
76  vec.z = atan2(2 * (quat.x*quat.y + quat.w*quat.z), squ + sqx - sqy - sqz);
77 
78  return vec;
79 }
80 
81 
83 {
84  public:
85 
87 
89 
90  nav_msgs::Odometry odom;
91 
93  {
94  };
95 
96 };
97 
98 int main( int argc, char** argv )
99 {
100 
101  /*********** Initialize ROS ****************/
102  ros::init(argc,argv);
103  ros::Node *node = new ros::Node("test_run_base_controller");
104 
105  test_run_base tb;
106 
107  node->subscribe("odom",tb.odom,&test_run_base::odomMsgReceived,&tb,10);
108 
109  signal(SIGINT, finalize);
110  signal(SIGQUIT, finalize);
111  signal(SIGTERM, finalize);
112 
113 
114  /*********** Start moving the robot ************/
115  geometry_msgs::Twist cmd;
116  cmd.linear.x = 0;
117  cmd.linear.y = 0;
118  cmd.linear.z = 0;
119  cmd.angular.x = 0;
120  cmd.angular.y = 0;
121  cmd.angular.z = 0;
122 
123  double run_time = 0;
124  bool run_time_set = false;
125  int file_num = 0;
126 
127  if(argc >= 2)
128  cmd.linear.x = atof(argv[1]);
129 
130  if(argc >= 3)
131  cmd.linear.y = atof(argv[2]);
132 
133  if(argc >= 4)
134  cmd.angular.z = atof(argv[3]);
135 
136  if(argc >=5)
137  {
138  run_time = atof(argv[4]);
139  run_time_set = true;
140  }
141 
142  if(argc ==6)
143  {
144  file_num = atoi(argv[5]);
145  }
146 
147  node->advertise<geometry_msgs::Twist>("cmd_vel",10);
148  sleep(1);
149 
150  libTF::Vector ang_rates;
151  ros::Time start_time = ros::Time::now();
152  ros::Duration sleep_time = ros::Duration().fromSec(0.01);
153 
154  std::ofstream odom_log_file;
155  odom_log_file.open("odom_log.txt");
156 
157 
158  while(!done)
159  {
160  ros::Duration delta_time = ros::Time::now() - start_time;
161 
162  if(run_time_set && delta_time.toSec() > run_time)
163  break;
164 
165  odom_log_file << tb.odom.pose.pose.position.x << " " << tb.odom.pose.pose.position.y << " " << tf::getYaw(tb.odom.pose.pose.orientation) << " " << tb.odom.twist.twist.linear.x << " " << tb.odom.twist.twist.linear.y << " " << tb.odom.twist.twist.angulat.z << " " << tb.odom.header.stamp.sec + tb.odom.header.stamp.nsec/1.0e9 << std::endl;
166 
167  cout << endl << "odometry:: " << endl << "velocity:" << endl << " x:" << tb.odom.twist.twist.linear.x << endl << " y:" << tb.odom.twist.twist.linear.y << endl << " omega:" << tb.odom.twist.twist.angular.z << std::endl;
168  node->publish("cmd_vel",cmd);
169  sleep_time.sleep();
170  }
171 
172  node->unsubscribe("odom");
173  node->unadvertise("cmd_vel");
174 
175  odom_log_file.close();
176 
177 
178 }
static double getYaw(const Quaternion &bt_q)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Duration & fromSec(double t)
nav_msgs::Odometry odom
Definition: test_odom.cpp:90
INLINE Rall1d< T, V, S > asin(const Rall1d< T, V, S > &x)
void finalize(int donecare)
Definition: test_odom.cpp:45
libTF::Vector GetAsEuler(geometry_msgs::Quaternion quat)
Definition: test_odom.cpp:53
void odomMsgReceived()
Definition: test_odom.cpp:92
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
int main(int argc, char **argv)
Definition: test_odom.cpp:98
static Time now()
static int done
Definition: test_odom.cpp:43


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Mon Jun 10 2019 14:26:33