void BodyKinematics::teleopBodyCmd(const crab_msgs::BodyCommandConstPtr &body_cmd){ if (body_cmd->cmd == body_cmd->STAND_UP_CMD){ ROS_ERROR("STAND_UP_CMD"); ros::Rate r(25); while (bs.z >= -z){ bs.z -= 0.0025; r.sleep(); if (calculateKinematics(&bs)){ joints_pub.publish(legs); } } } if (body_cmd->cmd == body_cmd->SEAT_DOWN_CMD){ ROS_ERROR("SEAT_DOWN_CMD"); ros::Rate r(25); while (bs.z <= -0.016){ bs.z += 0.0025; r.sleep(); if (calculateKinematics(&bs)){ joints_pub.publish(legs); } } } // if (body_cmd->cmd == body_cmd->IMU_START_CMD){ // ROS_ERROR("IMU_START_CMD"); // ros::Rate r(25); // while (bs.z >= -0.08){ // bs.z -= 0.0025; // r.sleep(); // if (calculateKinematics(&bs)){ // joints_pub.publish(legs); // } // } // } }
bool BodyKinematics::init() { std::string robot_desc_string; // Get URDF XML if (!node.getParam("robot_description", robot_desc_string)) { ROS_FATAL("Could not load the xml from parameter: robot_description"); return false; } // Get Root and Tip From Parameter Server node.param("root_name_body", root_name, std::string("leg_center")); node.param("tip_name_body", tip_name, std::string("coxa")); node.param("clearance", z, 0.045); // Load and Read Models if (!loadModel(robot_desc_string)) { ROS_FATAL("Could not load models!"); return false; } client = node.serviceClient<crab_msgs::GetLegIKSolver>("/crab_leg_kinematics/get_ik"); joints_pub = node.advertise<crab_msgs::LegsJointsState>("joints_to_controller", 1); body_move_sub = node.subscribe<crab_msgs::BodyState>("/teleop/move_body", 1, &BodyKinematics::teleopBodyMove, this); body_cmd_sub = node.subscribe<crab_msgs::BodyCommand>("/teleop/body_command", 1, &BodyKinematics::teleopBodyCmd, this); bs.leg_radius = 0.11; bs.z = -0.016; ros::Duration(1).sleep(); if (calculateKinematics(&bs)){ joints_pub.publish(legs); } ROS_INFO("Ready to receive teleop messages... "); return true; }
void BodyKinematics::teleopBodyMove(const crab_msgs::BodyStateConstPtr &body_state){ bs.x = body_state->x; bs.y = body_state->y; bs.z = body_state->z; bs.pitch = body_state->pitch; bs.roll = body_state->roll; bs.yaw = body_state->yaw; bs.leg_radius = body_state->leg_radius; if (calculateKinematics(&bs)){ joints_pub.publish(legs); } }
//void BodyKinematics::teleopBodyMove(const spider_msgs::BodyStateConstPtr &body_state){ void BodyKinematics::teleopBodyMove(const spider_msgs::msg::BodyState::SharedPtr &body_state){ bs.x = body_state->x; bs.y = body_state->y; bs.z = body_state->z; bs.pitch = body_state->pitch; bs.roll = body_state->roll; bs.yaw = body_state->yaw; bs.leg_radius = body_state->leg_radius; if (calculateKinematics(&bs)){ //joints_pub.publish(legs); //joints_pub->publish(legs); } }
//void BodyKinematics::teleopBodyCmd(const spider_msgs::BodyCommandConstPtr &body_cmd){ void BodyKinematics::teleopBodyCmd(const spider_msgs::msg::BodyCommand::SharedPtr &body_cmd){ if (body_cmd->cmd == body_cmd->STAND_UP_CMD){ // ROS_ERROR("STAND_UP_CMD"); // ros::Rate r(25); rclcpp::rate::Rate loop_rate(25); while (bs.z >= -z){ bs.z -= 0.0025; //r.sleep(); if (calculateKinematics(&bs)){ //joints_pub.publish(legs); } } } if (body_cmd->cmd == body_cmd->SEAT_DOWN_CMD){ // ROS_ERROR("SEAT_DOWN_CMD"); // ros::Rate r(25); while (bs.z <= -0.016){ bs.z += 0.0025; //r.sleep(); if (calculateKinematics(&bs)){ //joints_pub.publish(legs); } } } if (body_cmd->cmd == body_cmd->IMU_START_CMD){ // ROS_ERROR("IMU_START_CMD"); // ros::Rate r(25); while (bs.z >= -0.08){ bs.z -= 0.0025; // r.sleep(); if (calculateKinematics(&bs)){ // joints_pub.publish(legs); } } } }
bool BodyKinematics::init() { std::string robot_desc_string; // Get URDF XML //if (!node.getParam("robot_description", robot_desc_string)) { // ROS_FATAL("Could not load the xml from parameter: robot_description"); // return false; // } // Get Root and Tip From Parameter Server //node.param("root_name_body", root_name, std::string("leg_center")); //node.param("tip_name_body", tip_name, std::string("coxa")); //node.param("clearance", z, 0.045); // Load and Read Models if (!loadModel(robot_desc_string)) { // ROS_FATAL("Could not load models!"); return false; } //client = node.serviceClient<spider_msgs::GetLegIKSolver>("/spider_leg_kinematics/get_ik"); //auto client = node->create_client<spider_msgs::srv::GetLegIKSolver>("/spider_leg_kinematics/get_ik"); //joints_pub = node.advertise<spider_msgs::LegsJointsState>("joints_to_controller", 1); //auto joints_pub = node->create_publisher<spider_msgs::msg::LegsJointsState>("joints_to_controller", rmw_qos_profile_default); //body_move_sub = node.subscribe<spider_msgs::BodyState>("/teleop/move_body", 1, &BodyKinematics::teleopBodyMove, this); //auto body_move_sub = node->create_subscription<spider_msgs::msg::BodyState>("/teleop/move_body", rmw_qos_profile_default, &BodyKinematics::teleopBodyMove, this); //body_cmd_sub = node.subscribe<spider_msgs::BodyCommand>("/teleop/body_command", 1, &BodyKinematics::teleopBodyCmd, this); //auto body_cmd_sub = node->create_subscription<spider_msgs::msg::BodyCommand>("/teleop/body_command", rmw_qos_profile_default, &BodyKinematics::teleopBodyCmd, this); bs.leg_radius = 0.11; bs.z = -0.016; //ros::Duration(1).sleep(); if (calculateKinematics(&bs)){ // joints_pub.publish(legs); //joints_pub->publish(legs); } //ROS_INFO("Ready to receive teleop messages... "); printf("%s\n","Ready to receive teleop messages... "); return true; }