00001 #include <kobuki_softnode/fake_kobuki.h>
00002
00003 namespace kobuki
00004 {
00005 void FakeKobuki::init(ros::NodeHandle& nh)
00006 {
00007 this->wheel_speed_cmd[LEFT] = 0.0;
00008 this->wheel_speed_cmd[RIGHT] = 0.0;
00009
00010
00011 double pcov[36] = { 0.1, 0, 0, 0, 0, 0,
00012 0, 0.1, 0, 0, 0, 0,
00013 0, 0, 1e6, 0, 0, 0,
00014 0, 0, 0, 1e6, 0, 0,
00015 0, 0, 0, 0, 1e6, 0,
00016 0, 0, 0, 0, 0, 0.2};
00017 memcpy(&(this->odom.pose.covariance),pcov,sizeof(double)*36);
00018 memcpy(&(this->odom.twist.covariance),pcov,sizeof(double)*36);
00019
00020
00021 this->wheel_separation = 0.23;
00022 this->wheel_diameter = 0.070;
00023
00024
00025 nh.param("wheel_left_joint_name",this->wheel_joint_name[LEFT], std::string("wheel_left_joint"));
00026 nh.param("wheel_right_joint_name",this->wheel_joint_name[RIGHT], std::string("wheel_right_joint"));
00027 nh.param("cmd_vel_timeout",this->cmd_vel_timeout, 0.6);
00028 this->cmd_vel_timeout = 1.0;
00029
00030 this->motor_enabled = true;
00031
00032 this->joint_states.header.frame_id = "Joint States";
00033 this->joint_states.name.push_back(wheel_joint_name[LEFT]);
00034 this->joint_states.name.push_back(wheel_joint_name[RIGHT]);
00035 this->joint_states.position.resize(2,0.0);
00036 this->joint_states.velocity.resize(2,0.0);
00037 this->joint_states.effort.resize(2,0.0);
00038
00039
00040 nh.param("odom_frame",this->odom.header.frame_id,std::string("odom"));
00041 nh.param("base_frame",this->odom.child_frame_id,std::string("base_footprint"));
00042
00043 this->versioninfo.hardware = "dumb";
00044 this->versioninfo.firmware = "fake";
00045 this->versioninfo.software = "0.0.0";
00046
00047 this->odom_pose[0] = 0;
00048 this->odom_pose[1] = 0;
00049 this->odom_pose[2] = 0;
00050
00051 }
00052 }