-
Notifications
You must be signed in to change notification settings - Fork 242
/
move_publisher.cpp
80 lines (66 loc) · 2.37 KB
/
move_publisher.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#include <ros/ros.h>
#include <gazebo_msgs/ModelState.h>
#include <gazebo_msgs/SetModelState.h>
#include <string>
#include <stdio.h>
#include <tf/transform_datatypes.h>
// #include <std_msgs/Float64.h>
#include <math.h>
#include <iostream>
int main(int argc, char **argv)
{
enum coord
{
WORLD,
ROBOT
};
coord def_frame = coord::WORLD;
ros::init(argc, argv, "move_publisher");
ros::NodeHandle nh;
ros::Publisher move_publisher = nh.advertise<gazebo_msgs::ModelState>("/gazebo/set_model_state", 1000);
gazebo_msgs::ModelState model_state_pub;
std::string robot_name;
ros::param::get("/robot_name", robot_name);
std::cout << "robot_name: " << robot_name << std::endl;
model_state_pub.model_name = robot_name + "_gazebo";
ros::Rate loop_rate(1000);
if(def_frame == coord::WORLD)
{
model_state_pub.pose.position.x = 0.0;
model_state_pub.pose.position.y = 0.0;
model_state_pub.pose.position.z = 0.5;
model_state_pub.pose.orientation.x = 0.0;
model_state_pub.pose.orientation.y = 0.0;
model_state_pub.pose.orientation.z = 0.0;
model_state_pub.pose.orientation.w = 1.0;
model_state_pub.reference_frame = "world";
long long time_ms = 0; //time, ms
const double period = 5000; //ms
const double radius = 1.5; //m
tf::Quaternion q;
while(ros::ok())
{
model_state_pub.pose.position.x = radius * sin(2*M_PI*(double)time_ms/period);
model_state_pub.pose.position.y = radius * cos(2*M_PI*(double)time_ms/period);
model_state_pub.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, - 2*M_PI*(double)time_ms/period);
move_publisher.publish(model_state_pub);
loop_rate.sleep();
time_ms += 1;
}
}
else if(def_frame == coord::ROBOT)
{
model_state_pub.twist.linear.x= 0.02; //0.02: 2cm/sec
model_state_pub.twist.linear.y= 0.0;
model_state_pub.twist.linear.z= 0.08;
model_state_pub.twist.angular.x= 0.0;
model_state_pub.twist.angular.y= 0.0;
model_state_pub.twist.angular.z= 0.0;
model_state_pub.reference_frame = "base";
while(ros::ok())
{
move_publisher.publish(model_state_pub);
loop_rate.sleep();
}
}
}