where i can find information about to how create a node to move a model in rviz using state-publisher.cpp

asked 2019-09-19 16:48:07 -0500

tegusg gravatar image

i have a scrip in c++ to move an robot model and visualized in rviz but i need more information because i don't understand the code.

#include <string>

include <ros ros.h="">

include <sensor_msgs jointstate.h="">

include <tf transform_broadcaster.h="">

int main(int argc, char** argv) { ros::init(argc, argv, "state_publisher"); ros::NodeHandle n; ros::Publisher joint_pub = n.advertise<sensor_msgs::jointstate>("joint_states", 1); tf::TransformBroadcaster broadcaster; ros::Rate loop_rate(30);

const double degree = M_PI/180;

// robot state
double inc= 0.05, base_link1_inc= 0.05, arm1_armbase_inc= 0.05, arm2_arm1_inc= 0.05, gripper_inc= 0.05, tip_inc= 0.05;
double angle= 0 ,base_arm = 0, arm1_armbase = 0, arm2_arm1 = 0, gripper = 0, tip = 0;
// message declarations
geometry_msgs::TransformStamped odom_trans;
sensor_msgs::JointState joint_state;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

while (ros::ok()) {
    //update joint_state
    joint_state.header.stamp = ros::Time::now();
    joint_state.name.resize(11);
    joint_state.position.resize(11);
    joint_state.name[0] ="base_to_arm_base";
    joint_state.position[0] = base_arm;
    joint_state.name[1] ="arm_1_to_arm_base";
    joint_state.position[1] = arm1_armbase;
    joint_state.name[2] ="arm_2_to_arm_1";
    joint_state.position[2] = arm2_arm1;
joint_state.name[3] ="left_gripper_joint";
    joint_state.position[3] = gripper;
joint_state.name[4] ="left_tip_joint";
    joint_state.position[4] = tip;
joint_state.name[5] ="right_gripper_joint";
    joint_state.position[5] = gripper;
joint_state.name[6] ="right_tip_joint";
    joint_state.position[6] = tip;
joint_state.name[7] ="base_to_wheel1";
    joint_state.position[7] = 0;
joint_state.name[8] ="base_to_wheel2";
    joint_state.position[8] = 0;
joint_state.name[9] ="base_to_wheel3";
    joint_state.position[9] = 0;
joint_state.name[10] ="base_to_wheel4";
    joint_state.position[10] = 0;






    // update transform
    // (moving in a circle with radius)
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.transform.translation.x = cos(angle);
    odom_trans.transform.translation.y = sin(angle);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);

    //send the joint state and transform
    joint_pub.publish(joint_state);
    broadcaster.sendTransform(odom_trans);


// Create new robot state
    arm2_arm1 += arm2_arm1_inc;
    if (arm2_arm1<-1.5 || arm2_arm1>1.5) arm2_arm1_inc *= -1;
      arm1_armbase += arm1_armbase_inc;
    if (arm1_armbase>1.2 || arm1_armbase<-1.0) arm1_armbase_inc *= -1;
    base_arm += base_arm_inc;
    if (base_arm>1. || base_arm<-1.0) base_arm_inc *= -1;
    gripper += gripper_inc;
    if (gripper<0 || gripper>1) gripper_inc *= -1;

      angle += degree/4;

    // This will adjust as needed per iteration
    loop_rate.sleep();
}


return 0;

}

edit retag flag offensive close merge delete

Comments

You may want to clean up your formatting a bit so the code is readable. I would also be more specific about what you don't understand. That's a lot of code to explain, if you narrow your question down a bit you're more likely to get an answer.

kscottz gravatar image kscottz  ( 2019-09-19 17:11:36 -0500 )edit

i don't search whole the explaination but i would like some guide o previous information about this because i don't understand many many things

tegusg gravatar image tegusg  ( 2019-09-19 17:34:55 -0500 )edit

Please consider following the suggestion from @kscottz. Which parts are not clear to you?

ffusco gravatar image ffusco  ( 2019-09-20 01:01:36 -0500 )edit