25 ROS_INFO(
"TurtleBot3 Simulation Node Init");
42 std::string cmd_vel_topic_name =
nh_.
param<std::string>(
"cmd_vel_topic_name",
"");
64 double siny = 2.0 * (msg->pose.pose.orientation.w * msg->pose.pose.orientation.z + msg->pose.pose.orientation.x * msg->pose.pose.orientation.y);
65 double cosy = 1.0 - 2.0 * (msg->pose.pose.orientation.y * msg->pose.pose.orientation.y + msg->pose.pose.orientation.z * msg->pose.pose.orientation.z);
72 uint16_t scan_angle[3] = {0, 30, 330};
74 for (
int num = 0; num < 3; num++)
76 if (std::isinf(msg->ranges.at(scan_angle[num])))
82 scan_data_[num] = msg->ranges.at(scan_angle[num]);
89 geometry_msgs::Twist cmd_vel;
91 cmd_vel.linear.x = linear;
92 cmd_vel.angular.z = angular;
102 static uint8_t turtlebot3_state_num = 0;
104 switch(turtlebot3_state_num)
162 int main(
int argc,
char* argv[])
164 ros::init(argc, argv,
"turtlebot3_drive");
void laserScanMsgCallBack(const sensor_msgs::LaserScan::ConstPtr &msg)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define TB3_DRIVE_FORWARD
int main(int argc, char *argv[])
double check_forward_dist_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::Publisher cmd_vel_pub_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void odomMsgCallBack(const nav_msgs::Odometry::ConstPtr &msg)
#define GET_TB3_DIRECTION
ROSCPP_DECL void shutdown()
void updatecommandVelocity(double linear, double angular)
ros::Subscriber odom_sub_
ros::Subscriber laser_scan_sub_
ROSCPP_DECL void spinOnce()