3 #include <geometry_msgs/TransformStamped.h> 4 #include <geometry_msgs/Twist.h> 5 #include <turtlesim/Spawn.h> 7 int main(
int argc,
char** argv){
15 turtlesim::Spawn turtle;
18 turtle.request.theta = 0;
19 turtle.request.name =
"turtle2";
23 node.
advertise<geometry_msgs::Twist>(
"turtle2/cmd_vel", 10);
30 geometry_msgs::TransformStamped transformStamped;
32 transformStamped = tfBuffer.lookupTransform(
"turtle2",
"turtle1",
41 geometry_msgs::Twist vel_msg;
43 vel_msg.angular.z = 4.0 *
atan2(transformStamped.transform.translation.y,
44 transformStamped.transform.translation.x);
45 vel_msg.linear.x = 0.5 *
sqrt(
pow(transformStamped.transform.translation.x, 2) +
46 pow(transformStamped.transform.translation.y, 2));
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
int main(int argc, char **argv)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)