27 #ifndef _atlascar_transforms_CPP_
28 #define _atlascar_transforms_CPP_
44 #include <sensor_msgs/JointState.h>
45 #include <tf/transform_broadcaster.h>
47 int main(
int argc,
char **argv)
49 ros::init(argc, argv,
"atlascar_transforms");
52 ros::Rate loop_rate(1);
54 tf::Vector3 V(-1.8583, 0, 0.9927);
55 tf::Matrix3x3 M(0.9744, 0, 0.2250, 0, 1, 0, -0.225, 0, 0.9744);
57 #if ROS_VERSION_MINIMUM(1, 8, 0) // if current ros version is >= 1.8.0
58 M.getEulerYPR(y, p, r);
59 #else // Code for earlier versions (Electric, diamondback, ...)
62 ROS_INFO(
"Link position (to copy paste directly to urdf)");
63 ROS_INFO(
"xyz=\"%f %f %f\" rpy=\"%f %f %f\"", V[0], V[1], V[2], r,p,y);
66 tf::TransformBroadcaster tf_br1;
67 tf::Transform tr1 = tf::Transform(tf::Matrix3x3(0.8039, 0.5948, 0, -0.5948, 0.8039, 0, 0, 0, 1),
68 tf::Vector3(-0.08, 0.8350, 0));
71 tf::TransformBroadcaster tf_br2;
72 tf::Transform tr2 = tf::Transform(tf::Matrix3x3(-0.8192,0.5736,0,-0.5736,-0.8192,0,0,0,1),
73 tf::Vector3(-0.12, -0.89, -0.0950));
76 tf::TransformBroadcaster tf_br3;
77 tf::Transform tr3 = tf::Transform(tf::Matrix3x3(0.9890,0.0129,0.1472,0.,0.9962,-0.0872,-0.1478,0.0862,0.9853),
78 tf::Vector3(-1.7803, 0, 1.1861));
95 ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>(
"joint_states", 1);
96 tf::TransformBroadcaster broadcaster;
98 const double degree = M_PI/180;
101 double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;
104 geometry_msgs::TransformStamped odom_trans;
105 sensor_msgs::JointState joint_state;
106 odom_trans.header.frame_id =
"odom";
107 odom_trans.child_frame_id =
"axis";
111 joint_state.header.stamp = ros::Time::now();
112 joint_state.name.resize(3);
113 joint_state.position.resize(3);
114 joint_state.name[0] =
"swivel";
115 joint_state.position[0] = swivel;
116 joint_state.name[1] =
"tilt";
117 joint_state.position[1] = tilt;
118 joint_state.name[2] =
"periscope";
119 joint_state.position[2] = height;
124 odom_trans.header.stamp = ros::Time::now();
125 odom_trans.transform.translation.x = cos(angle)*2;
126 odom_trans.transform.translation.y = sin(angle)*2;
127 odom_trans.transform.translation.z = .7;
128 odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
131 joint_pub.publish(joint_state);
132 broadcaster.sendTransform(odom_trans);
136 if (tilt<-.5 || tilt>0) tinc *= -1;
138 if (height>.2 || height<0) hinc *= -1;
152 tf_br1.sendTransform(tf::StampedTransform(tr1, ros::Time::now(),
"/tf_atc/vehicle",
"/tf_atc/laser/left_bumper"));
153 tf_br2.sendTransform(tf::StampedTransform(tr2, ros::Time::now(),
"/tf_atc/vehicle",
"/tf_atc/laser/right_bumper"));
154 tf_br3.sendTransform(tf::StampedTransform(tr3, ros::Time::now(),
"/tf_atc/vehicle",
"/tf_atc/laser/roof_rotating_base"));
_EXTERN_ ros::NodeHandle * p_node
int main(int argc, char **argv)
Deprecated example header.